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ABSTRACT 


Feedback  control  of  a  two  wheeled  mobile  robot  from  one  point  in  its 
configuration  space  to  another  presents  a  challenging  problem.  The  mobile  robot 
belongs  to  a  class  of  systems  with  non-integrable  motion  constraints  for  which 
smooth  feedback  control  laws  cannot  be  designed.  Recent  work  has  been  aimed 
at  develc^ing  time-varying  feedback  control  laws  and  piecewise  smooth  feedback 
control  laws.  These  control  techniques  are,  however,  not  optimal  in  any  sense. 
In  this  research,  we  look  into  the  optimal  contrcrf  of  a  mobile  robot  using  partial 
feedback.  A  solution  is  obtained  by  a{^lication  of  Pontryagin’s  Minimization 
Priciple  and  solving  the  associated  two  point  boundary  value  problem  using  a 
numerical  relaxation  technique.  The  resulting  rob<H  trajectories  exhibit  optimal 
behavior  for  all  non-trivial  cases. 
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I. 


INTRODUCTION 


The  mobile  robot  belongs  to  a  class  of  systems  with  non- 
integrable  motion  constraints  for  which  smooth  feedback 
control  laws  for  motion  from  one  point  in  the  configuration 
space  to  another  cannot  be  designed  [Ref.  1].  Recent  work 
aims  at  developing  time-varying  feedback  control  laws  [Ref.  1] 
and  piecewise  smooth  feedback  control  laws  [Ref.  2].  These 
control  techniques  are,  however,  not  optimal  in  any  sense.  In 
this  research,  we  look  into  the  optimal  control  of  a  mobile 
robot  using  partial  feedback. 

A.  KINEMATICS  OP  A  MOBILE  ROBOT 

The  position  and  orientation  of  a  two  wheeled  mobile  robot 
on  a  horizontal  plane  is  described  by  three  generalized 
coordinates.  Figure  l  shows  the  three  coordinates  chosen  for 
our  robot.  These  are  the  two  X-Y  coordinates  for  the  location 
of  the  robot  on  the  plane,  and  an  angular  displacement,  6,  to 
describe  the  robot's  orientation  with  respect  to  the  positive 
X  axis. 

The  velocity  of  the  robot  can  be  described  completely  in 
terms  of  translation  and  rotation.  Assuming  no  slipping,  the 
interaction  of  the  wheels  with  the  plane  restricts  the 
instantaneous  motion  to  the  direction  of  orientation  of  the 
robot.  Defining  as  the  velocity  in  the  direction  of 
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orientation,  and  Uj  as  the  rate  of  change  of  the  orientation, 
the  following  constraint  equations  result: 


X  =  cose 


(1) 


y  =  sine 


(2) 


(3) 


Note  that  while  the  constraints  ed>ove  limit  the  nxmiber  of 
degrees  of  freedom  for  the  system  to  two,  specifically  and 
U21  a  minimum  of  three  coordinates  are  required  to  describe 
the  system.  This  is  true  of  all  nonholonomic  systems;  the 
number  of  generalized  coordinates  required  to  describe  the 
system  is  greater  than  the  number  of  degrees  of  freedom. 

A  nonholonc^ic  system  is  characterized  by  the  non- 
integraUsle  nature  of  the  constraint  between  the  first 
derivatives  of  the  coordinates.  [Ref.  4:p.  244]  In  the 
particular  case  of  the  mobile  robot,  the  non- integrable 
constraint  is  due  to  the  nature  of  the  angular  displacement 
term,  B.  As  0  is  an  Independent  function  of  time,  the 
relationship  between  the  remaining  coordinates  Ccuinot  be 
uniquely  determined.  In  other  words,  for  a  robot  moving  from 
one  position  and  orientation  on  the  X-Y  plane  to  another,  the 
instantaneous  value  of  B  depends  upon  the  path  followed  by  the 
robot.  As  a  result,  the  coordinate  relationship  is  dependent 
upon  the  path  taken. 
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B .  OPTIMAL  CONTROL 


Since  the  number  of  paths  the  robot  could  follow  is 
infinite,  some  paths  would  be  more  efficient  than  others.  In 
order  to  determine  the  most  efficient  path,  we  must  first 
chose  a  cost  function  or  a  performance  index.  Following  the 
development  in  Reference  5,  pp.  180-183,  for  optimal  control 
of  a  standard  nonlinear  system,  we  may  obtain  the  necessary 
conditions  for  optimality. 

We  first  express  the  differential  equations  of  motion  in 
the  form, 

X  =  f(x,  u,  t)  (4) 

The  cost  function  can  take  the  form 

J  =  $  [x(  tf)  ,  tf]  +  t)  ,  u(  t)  ,  t]  dt  (5) 

where  F  could  represent  the  pseudo- kinetic  energy  in  the  form 
,  with  u  as  the  velocity.  The  term  is  a  terminal  cost 
vector  and  is  a  function  of  the  states  at  the  final  time. 
This  final  time  is  not  specified.  Applying  Lagrange 
multiplier  vector,  X,  we  form  the  augmented  functional. 

[F+k'^(  f-x)  ]  dt  (6) 

to 

After  defining  the  Hamiltonian  as 

H=F*X'^f  (7) 

we  can  determine  the  necessary  conditions  for  an  optimal 

solution  using  Pontryagin's  Minimization  Principle: 

[Ref.  5:p.  183] 
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(8) 

(9) 

dx 

(10) 

(11) 

^-0 

OU 

(12) 

"The  optimal  control  u(t)  is  determined  at  each  instant  to 
render  the  Hamiltonian  a  minimum  over  all  admissible  control 
functions,"  [Ref.  5;p.  183]  Using  the  last  condition,  we  can 
solve  for  the  control  input,  u,  in  terms  of  the  states,  x,  and 
what  we  will  now  refer  to  as  costates,  X. 

Let  us  now  consider  some  simplifications  to  the  above 
necessary  conditions.  If  we  fix  the  final  time  to  achieve  the 
desired  condition,  the  first  criterion  is  immediately 
satisfied  as  6tf  =  0.  If  we  also  describe  the  desired 
condition  directly  in  terms  of  the  states,  x,  and  fix  the 
value  of  the  desired  final  states,  then  6x(tf)  =  0. 
Consequently,  the  second  condition  is  met.  In  practical 
terms,  this  translates  to  going  to  a  desired  set  of  states  in 
a  fixed  amount  of  time. 

We  now  apply  these  simplifications  to  the  differential 
expressions  for  the  states  and  costates.  Assuming  our  initial 
states  are  known,  we  have  boundary  conditions  for  the  states 
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at  the  initial  and  final  time.  However,  we  know  neither  the 
initial  or  final  boundary  conditions  for  the  costates.  And 
since  the  state  and  costate  differential  expressions  are 
coupled,  they  must  be  solved  simultaneously.  As  a  result,  the 
combined  expressions  give  the  form  for  a  two  point  boundary 
value  problem. 


xl 

fix.k) 

i. 

g(x,X) 

(13) 


C.  TWO  POINT  BOUNDARY  VALUE  PROBLEMS 

In  the  case  of  linear  differential  equations,  many 
analytic  methods  are  available  for  solution  of  two  point 
boundary  value  problems.  However  for  nonlinear  problems  like 
the  mobile  robot,  analytic  methods  for  the  solution  to  the  two 
point  boundary  value  problem  do  no  exist.  In  some  cases,  non¬ 
linear  problems  can  be  solved  analytically.  Such  problems  are 
generally  very  simple  and  may  only  represent  special  cases  of 
an  overall  problem.  As  we  shall  see  later,  the  mobile  robot 
problem  does  not  lend  itself  readily  to  analytic  methods. 

In  many  cases,  a  non-linear  two  point  boundary  value 
problem  can  best  be  solved  numerically.  Unfortunately, 
numerical  methods  for  non-linear  two  point  boundary  value 
problems  are  usually  fairly  complicated. 
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The  general  approach  is  to  make  an  initial  "guess"  to  the 
solution  and  adjust  this  trial  solution  to  match  the  boundary 
conditions  and  differential  equations.  There  are  two  distinct 
methods  for  solving  such  problems,  shooting  and  relaxation 
[Ref.  6], 

The  first  method,  shooting,  requires  an  initial  guess  of 
dependent  variables  based  upon  one  boundary.  Then  using 
numerical  methods  common  to  initial  value  problems,  we  obtain 
a  trial  solution.  This  trial  solution  is  compared  against  the 
second  boundary.  The  error  between  the  two  is  noted  and  the 
free  parameters  of  the  equation  adjusted  accordingly.  This 
repeats  until  the  error  is  sufficiently  small.  The  advantages 
of  this  method  are  its  simplicity  and  relative  speed.  For 
extremely  non-linear  systems,  however,  systematically 
improving  the  solution  can  prove  difficult. 

In  the  second  method,  relaxation,  the  differential 
equations  are  converted  into  difference  expressions  using 
Taylor  series  expansion.  With  an  arbitrary  initial  trial 
solution,  the  variance  of  each  point  in  the  discretized  mesh 
is  calculated.  The  trial  solution  is  then  adjusted  to  improve 
agreement  with  the  differential  equations  and  the  boundary 
conditions.  This  continues  iteratively  until  the  variance,  or 
error,  of  the  solution  is  sufficiently  small.  Relaxation 
methods  are  considered  advantageous  for  problems  with 
complicated  boundary  conditions,  but  smooth  and  non- 
oscillatory  functions.  Two  disadvantages  of  this  method  are 
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the  large  number  of  variables  to  be  solved  simultaneously  and 
complexity  of  the  expressions  required  in  the  algorithm.  The 
number  of  differential  equations,  mesh  size  and  coupling  of 
adjacent  points  in  the  mesh  determine  the  number  of  variables 
to  solve.  For  example,  in  a  system  with  8  differential 
equations,  on  a  mesh  with  100  points,  coupling  two  points, 
1600  variables  would  result. 

For  the  mobile  robot  problem,  the  kinematic  equations 
involve  trigonometric  functions.  As  we  shall  see  in  Chapter 
III,  the  resulting  state  and  costate  differential  equations 
are  highly  non-linear.  In  anticipation  of  the  highly  non¬ 
linear  kinematic  behavior  of  a  mobile  robot,  the  approach 
taken  here  is  the  relaxation  method.  We  take  advantage  of 
published  computer  programs  designed  specifically  for  this 
method . 

D.  APPLICATION  OF  THE  RELAXATION  METHOD 

As  previously  stated,  the  method  starts  with  an  initial 
guess  trajectory  for  each  of  the  differential  equations  and 
then  adjusts  these  trial  solutions  to  match  both  the  governing 
equations  and  the  boundary  conditions.  The  method  in  which 
the  computer  program  makes  the  corrections  to  the  trajectories 
is  a  key  to  finding  a  proper  solution.  The  source  of  the 
computer  code  and  expression  preparation  process  used  here  is 


Reference  6. 


Given  a  set  of  N  coupled  first  order  differential 
equations,  we  first  divide  the  independent  variable  domain 
into  M  discrete  mesh  points,  t^,  k-  1,2,  .  .M.  For  our  problem, 
the  initial  state  boundary  values  are  located  at  and  the 
final  state  boundary  values  at  The  costate  boundary 
values  are  not  fixed.  The  N  differential  equations  then 
become  finite  difference  equations  to  describe  the  internal 
meshpoints .  We  define  the  vector  yj^  as  the  entire  set  of 
dependent  variables  at  point  The  exact  form  of  the  finite 
difference  equation  depends  on  the  coupling  desired.  For  our 
purposes,  a  backward  difference  technique  is  sufficient.  This 
will  couple  each  point  on  the  mesh  with  the  point  preceding 
it. 

By  comparing  the  difference  between  adjacent  solution 
values,  (Yk'yk-i^  >  solution  of  the  finite  difference 
equations,  we  form  an  error  equation.  A  solution  exists  where 
the  error  equations  are  zero  and  the  boundary  conditions  are 
met.  Considering  any  internal  mesh  point,  k,  this  error 
expression  takes  the  form 

^k~y  k  y k-1  ^k-l'^^k^^k’  ^  (14) 

Through  Taylor  series  expansion  of  the  error  equation  we 
determine  the  variance  of  the  error  with  small  changes  in  Ayj^. 
Since  we  are  looking  for  the  solution  where  the  error  is  zero, 
for  the  internal  mesh  points,  k=2,3...M,  the  form  is 
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(15) 


i^^j.n^yn.k  1  *  53  ^j.n^yn  N.k  ^  _j .  k 


where 


j  =  1.2.  .  .  .N 


dE^  *  dE.  . 

C  -  J  •  ^  C*  -  J  t  ^ 

dv  '  aT;  ' 

'^yn.k-l  ^yn,k 

n=l ,2 ,  . .  . , N 


At  each  internal  point,  k,  Sj^n  forms  a.  N  X  2N  matrix.  The 
contents  of  this  matrix  are  corrections  to  the  solution 
variables  between  points  k  and  k-1. 

At  the  initial  boundary,  since  depends  only  on  the 


relation  takes  the  form 


j-nz*l,n.+2,  .  .  .  ,N 


where 


s 

(18) 

n=l, 2, . . . ,N 

And  similarly,  at  the  final  boundary,  where  depends 
only  on  the  form  is 

K 


J=l,  2,  .  .  .  ,n. 


where 


O  - 

dv 

^yn.M 


n=l,2, . .  . ,N 
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The  above  equations  can 

now  be  used 

to  solve 

for 

corrections.  Ay,  to 

the  trial 

solution  vector,  y. 

This 

process  continues  i 

terat ively 

until  the 

correct  ion 

are 

sufficiently  small. 

Of  course,  since  the 

equations 

are 

coupled,  they  must  be  solved  simultaneously. 

If  we  combine  the  expressions  for  each  internal  point  and 
boundary  points  in  a  global  matrix,  we  see  that  matrix  has  a 
special  "block  diagonal"  form  (Fig.  2) .  This  form  allows  a 
more  economical  matrix  inversion  process.  The  matrix 
inversion  is  accomplished  through  a  form  of  Gaussian 
elimination  which  takes  advantage  of  the  special  form.  This 
process  requires  significantly  less  computational  time  or 
storage  than  inversion  of  the  entire  matrix.  This  is  critical 
due  to  the  size  of  the  global  matrix,  (MN  X  MN) . 

Recall  that  our  overall  goal  is  to  determine  the  optimal 
trajectory  for  a  mobile  robot  traversing  from  one  position  and 
orientation  to  another.  Application  of  optimal  control  theory 
results  in  a  two  point  boundary  value  problem.  Using  the 
method  described  above,  we  can  solve  most  problems  of  this 
form.  However,  this  method  does  not  guarantee  a  solution. 
Many  factors  will  affect  the  program's  ability  to  converge  to 
a  solution.  Therefore  before  attempting  the  two  wheeled 
mobile  robot  problem,  a  simpler  related  problem  will  be 
solved.  This  will  serve  to  provide  insight  on  use  of  the 
program  and  validate  the  process. 
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II.  OPTIMAL  CONTROL  OP  A  ROLLING  DISK 


In  Chapter  I,  we  provided  an  outline  for  the  optimal 
control  problem  of  a  dynamical  system.  In  this  chapter  we 
apply  Pontryagin's  Minimization  Principle  [Ref.  5]  and  solve 
the  associated  two  point  boundary  value  problem  for  the  simple 
excunple  of  a  rolling  disk.  The  differential  equations  of 
motion  for  the  disk  and  robot  systems  are  similar,  and  the 
nonholonomic  constraint  is  exactly  the  same;  no  side  slipping 
is  allowed.  The  only  difference  between  the  rolling  disk  and 
the  mobile  robot  model  is  the  addition  of  a  state  variable: 
the  angular  orientation  of  the  rolling  disk  about  it's 
rotational  axis,  (p. 

A.  PROBLEM  DESCRIPTION 

Consider  a  vertical  disk  rolling  on  the  horizontal,  X-Y 
plane.  (Fig.  3).  Like  the  mobile  robot,  the  orientation  of 
this  disk  with  respect  to  the  plane  will  be  described  as  an 
angular  displacement,  6,  from  the  X  axis.  The  orientation  of 
the  disk  face  with  respect  to  it's  axis  of  rotation  is 
described  as  an  angular  displacement,  <t>,  from  the  normal 
vector  to  the  X-Y  plane.  This  gives  us  a  total  of  5 
coordinates  to  describe  the  position  and  orientation  of  the 
disk. 
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The  velocity  of  the  disk,  like  the  robot,  can  be  described 


in  terms  of  translation  and  rotation.  The  translational 
velocity  again  is  constrained  to  the  direction  of  orientation 
of  the  disk.  However,  the  forward  velocity  of  the  disk  is 
directly  related  to  the  angular  velocity  of  (f>  and  disk  radius, 
R.  If  we  consider  the  variation  of  6  and  4>  with  time  as 
control  inputs,  and  U2  respectively,  the  state- space  form 
of  the  kinematic  equations  becomes 


X 

Y 

e 


0  J?  COS0 
0  R  sin0 
1  0 
0  1 


0 


or  in  a  more  condensed  form  as 

^  =  Ik]  u 


(21) 


(22) 


B .  OPTIMAL  CONTROL 

The  objective  for  this  problem  is  to  roll  the  disk  from 
and  initial  position  and  orientation  to  a  desired  final 
position  and  orientation  in  some  optimal  manner.  Note  that 
for  our  problem  the  time  to  accomplish  this  task  is  fixed. 
The  choice  of  units  for  the  X-Y  parameters  are  arbitrary.  The 
angular  displacements  are  in  non-dimensional  radians.  Time  is 
considered  on  the  unity  scale  with  0  at  tg  to  1  at  tf.  The 
initial  conditions  at  tg  are  defined  as  Y^,  6^,  <t>^,  and  the 
final  conditions  at  tf  as  Xf,  Yf,  Of, 
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The  development  of  the  optimal  control  problem  follows  the 
method  described  in  Chapter  I.  To  determine  an  optimal  path 
for  the  disk,  we  define  the  performance  parameter  as 


^  \  ju^u  dc 

Cq 


(23) 


Since  the  terminal  costs  are  path  independent,  they  are 
neglected  here.  Adding  a  Lagrange  multiplier  the  cost 
functional  becomes 


J  =  f  (ju^u  *  X^(Ku  -  x))dt  (24) 

^0 


By  defining  the  Hamiltonian, 


H  =  j  (u^'u  +  X'^Ku)  (25) 

the  optimal  control  is  obtained  as: 

u  =  -K-^X  (26) 

Substituting  this  expression  into  equation  (25) ,  the 
Hcuniltonian  becomes 


(A.^  K  X) 


(27) 


Using  this  new  expression,  the  states  can  be  expressed  as 

X  =  -K  X  (28) 


or  in  expanded  form. 
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(29) 


\x 

Y 

e 


2~ 

2 


X 

(1  +  COS20)  -  — ^  sin20  -  RcosQk^ 

r2 

sin20  -  — ^  (1  -  COS20)  -  i?sin0X4 

-^3 

-(/?cos0Xj  +  /?sin0A,2  +  A,^) 


Similarly,  the  costates  equations  can  be  expressed  as 


X  =  -4-(--k'^KK^X) 

dx  2 


(30) 


Noting  that  the  matrix  K  is  only  a  function  of  state  variable 
0,  the  individual  costate  equations  become 


>^2 

^■3 


=  0 
=  0 


[(X|  -  Xi)  (sin20  +  2X^X2  +  cos20)]^ 

2 

+i?X4{X2Cos0  -  X2sin0) 


=  0 


(31) 


Combining  the  states  and  costates  into  a  single  vector  gives 
the  structure  for  the  two  point  boundary  value  problem. 
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X 

Y 

e 

4> 

^■3 


p2  1  p2  1 

- ^  (1  +  COS20)  -  - ^sin20  -  i?cos0X. 

2  2. 

p2 1  R^X 

- ^^sin20  -  - -  (1  -  cos20)  -  i?sin0X. 

2  2  ^ 

[-M 

[-(i?cos0A.^  +  i?sin0X2  +  X^)] 

0 

0 


^[(Xj-Xi)  sin20  +  2X^X3  +  cos20]  + 

RX^  (X2cos0-Xisin0) 

0 


(32) 


To  the  best  of  our  knowledge,  no  analytical  solution  exists  to 
this  problem.  A  similar  problem  has  been  solved  analytically 
by  Cameron  [Ref.  7].  However,  his  problem  looks  for  the 
minimum  time  solution.  By  use  of  Pontryagin's  Minimization 
Principle,  equation  (12)  ,  this  implies  use  of  the  time 
derivative  of  the  Hcimiltonian.  For  the  minimum  time  problem, 
it  can  be  shown  that  the  Hamiltonian  is  a  constant .  However 
in  our  problem,  the  final  time  is  fixed  and  terminal  cost, 
is  zero.  From  equation  (8) ,  the  Hamiltonian  may  therefore  be 
any  value  over  time.  Therefore,  Cameron's  analytical  method 
does  not  apply  to  our  fixed  time  problem. 


L5 


C.  NUMERICAL  SOLUTION  BY  THE  RELAXATION  METHOD 


Given  the  N  differential  equations  above,  we  apply  the 
relaxation  method  described  in  Chapter  I  to  develop 
expressions  required  by  the  relaxation  method  computer 
program.  This  essential  entails  finding  the  elements  of  the 
S  matrix.  For  the  interior  meshpoints,  a  total  of  N  X  2N  such 
expressions  must  be  developed.  The  two  boundaries  each 
require  an  additional  N  X  N  expressions.  Since  there  are 
eight  differential  equations,  we  must  develop  a  total  of  144 
Sj  „  expressions.  Fortunately,  many  of  the  expressions  for 
this  particular  problem  will  turn  out  to  be  zero. 

Rather  than  repeating  the  development  for  all  these 
expressions  here,  an  example  of  developing  expressions  for  an 
interior  point  is  presented.  Given  a  differential  equation 
which  describes  the  interior  mesh  points,  the  first  step  is  to 
substitute  for  all  dependent  variables,  where  n  is  the 
equation  number,  such  that 


Y-Y, 

d-Y, 

K-Ys 

K-Y, 

We  then  apply  the  finite  difference  expression 


(33) 


Y„  k*Y„  ^  , 

Y  ^  /J#Jr  13, /T"-! 
n,  k  o 


(34) 
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to  each  independent  variable.  Taking  the  first  state  equation 
as  an  example,  the  finite  difference  equation  is 


= 


(35) 


2  2V  2  ) 

2  2 


Next,  the  finite  difference  equation  is  placed  into  the  error 
expression. 


2  2  [  2  ) 
2  2  V  2  j 


-R  _ I  y^.k  ^ 


•COS  ( 


(36) 


Where  h  is  the  grid  spacing  on  the  mesh.  For  our  evenly 
spaced  mesh, 

h  =  (37) 

M-1 

As  given  by  equation  (16)  ,  the  ^j,n  expressions  are  the 
partial  derivatives  of  the  error  expressions  with  respect  to 
each  of  the  states  and  costates  at  meshpoint  k  and  k-l. 
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Again,  in  the  interest  of  brevity,  only  two 
expressions  are  presented  here.  Talcing  ^  as  the  first 

example  yields 

5i.5  "  (38) 

Fortunately,  due  to  the  finite  difference  method  chosen,  these 
expressions  tend  to  repeat.  For  example  yields 

(39) 

the  Scime  as  S2  5.  The  development  of  the  other  126  interior 
meshpoint  expressions  follow  similarly,  some  simpler  than 
others.  The  final  result  for  all  of  these  terms  can  be  seen 
in  the  DIFEQ.FOR  subroutine  in  Appendix  A. 

The  expressions  for  the  boundary  expressions,  though 
similar,  fall  under  equations  (18)  and  (20)  .  The  major 
difference  for  the  boundary  expressions  is  that  they  are  not 
based  on  the  differential  equations.  Since  our  boundary 
conditions  are  simply  state  values,  the  error  expressions  are 
at  the  initial  and  final  time  are  of  the  form 

where  n  is  the  nth  variable  as  given  by  equation  (33) .  Thus 
for  the  initial  and  final  boundary  conditions  respectively. 


dE^  1 

J-  ^  =  9 

ay  -'j*n^.n*N 

1 


=  c 

3y  *^j,n»N 


n,M 


(J=l,2, . 
(/I=l,  2,  . 


J  1,  for  j=ja  I 
I  0 ,  for  jVn  J 

.  .N) 


(41) 


where  N  is  the  total  number  equations  and  n^  is  the  number  of 
boundary  conditions  at  the  initial  time.  The  shift  in  indices 
by  N  and  n^  is  necessary  to  take  advantage  of  the  'block 
diagonal'  form  of  the  overall  matrix  of  S  expressions.  The 
result  is  the  unity  matrix  for  the  initial  and  final  S 
expressions.  Note  that  for  more  complicated  boundary 
conditions,  such  as  a  manifold  of  states  or  terminal  costs, 
the  relationships  above  are  not  valid. 

Next,  we  must  develop  an  initial  guess  for  the  values  of 
the  states  and  costates  for  all  points  on  the  mesh.  For  the 
states  this  guess  can  be  somewhat  intuitive.  For  example,  we 
desire  that  the  disk  start  at  the  X,Y  position  (0,0)  and  roll 
ten  (10)  times  and  make  one  (l)  complete  turn  to  return  to  the 
starting  position.  Therefore,  the  initial  and  final  boundary 
conditions  are 


Xq  =  0  Xf  =  Q 

^0  =  0  =  0 
00  =  0  6^  =  271 

<t>0  “  0  <i>f  =  2071 


(42) 


where  the  angular  terms  are  expressed  in  radians . 
Intuitively,  we  would  expect  the  most  optimal  path  in  the  X-Y 
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plane  to  be  a  circle.  If  we  initially  assume  that  the  angular 
terms  vary  at  a  constant  rate,  the  initial  guess  trajectory 
for  the  state  variables  will  appear  as  shown  in  Figure  4 . 
Since  we  have  no  information  on  the  costate  behavior,  we  will 
assume  the  initial  trajectory  for  each  costate  to  be  a 
constant  value  of  zero. 

After  the  required  expressions  and  initial  guess  entry 
method  is  successfully  compiled,  the  program  is  ready  to  run. 
A  sampling  of  the  results  follow. 


D.  DISCUSSION  OF  RESULTS 

For  the  case  described  above,  the  program  converges  in  a 
few  hundred  iterations.  From  Figure  5,  we  see  that  the  final 
state  solution  is  in  fact  the  same  as  the  initial  guess.  The 
iterations  were  required  to  adjust  the  costate  solutions  to 
their  proper  trajectories.  (Fig.  6)  Since  the  state  solution 
gives  the  expected  circular  path,  the  solution  appears  to  be 
optimal . 

For  a  more  rigorous  validation,  we  substitute  the  costate 
solutions 


Xi(t)  =0 
^2(0  =0 
X3  { t)  =-271 
( t)  =-2071 

back  into  equation  (32) . 


(43) 
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The  derivative  equations  can  then  be  expressed  as 


X  =  2OTrcos0 
Y  =  20usin6 
0  =  27t 
=  20il 
=  0 

K  =  0 

A3  =  0 

A4  =  0 


(44) 


Note  that  the  angular  velocity  tenns  arp  constant.  This  is 
consistent  with  the  minimization  of  our  cost  function.  And 
since  this  is  a  kinematics  problem,  the  velocities  may  be  non¬ 
zero  at  the  initial  and  final  time.  Integrating  the  state 
terms  yields 

X(t)  =lOsin0 
y(  t)  =10  (1-COS0) 

0(t)=2iit  '  ^ 

<|>(  t)  =207tt 


which  gives  the  equation  for  a  circle  in  the  X-Y  plane. 

If  we  make  a  slightly  different  initial  guess  for  the 
states,  such  as  an  ellipse  (Fig.  7)  the  final  result  is  the 
same.  If  however,  the  initial  guess  is  not  sufficiently  good, 
the  program  does  not  converge.  While  the  initial  guess  for 
the  states  can  usually  be  based  on  some  intuitive  reasoning, 
providing  a  sufficiently  good  estimate  of  the  costate  can 
prove  difficult.  For  this  problem,  an  initial  guess  of  all 
zeros  for  costates  works  quite  well.  If,  however,  we  chose 
trial  values  that  are  10  units  away  from  the  proper  solution, 


the  program  does  not  converge.  Thus,  while  the  costates  may 
not  be  particularly  important  to  the  usable  state  space 
solution,  they  are  necessary  to  solve  the  optimal  control 
problem.  Generally  though,  a  poor  estimation  of  the  costate 
can  be  con^ensated  for  by  a  good  state  estimation. 

Where  the  circular  path  presents  a  fairly  simple  solution, 
we  now  choose  a  more  difficult  task  for  our  disk.  This  will 
demonstrate  the  usefulness  of  this  method  for  problems  where 
the  optimal  path  is  not  obvious.  For  exan^le,  we  desire  that 
the  disk  make  10  rolls  and  5.5  turns  while  moving  on  the  X-Y 
eixis  form  a  point  (10,10)  to  a  point  (-5,-2).  As  an  initial 
guess,  we  shall  use  the  state  cuid  costate  solution  to  the 
circular  problem  cdiove.  The  resulting  path,  obtained  after 
several  hundred  iterations,  appears  in  Figure  8.  The  state 
and  costate  trajectories  appear  in  Figures  9  and  10, 
respectively.  While  these  solutions  appear  optimal,  they  are 
not  obvious  at  the  outset  of  the  problem. 

The  program  for  the  disk  problem  has  been  tested 
extensively  and,  when  provided  a  sufficiently  good  guess, 
found  to  give  an  apparently  optimal  solution  for  all  cases 
except  one.  For  the  case  of  rolling  the  disk  where  the 
initial  and  final  0  boundary  conditions  are  the  same  and  lie 
along  the  same  line,  the  program  does  not  converge.  However, 
the  program  will  converge  if  there  is  at  least  a  very  small 
difference  between  the  initial  and  final  angles.  For  the 
nearly  straight  line  case,  the  smallest  angular  difference 
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which  results  in  a  convergence  is  .00036  degrees.  (Fig.  11) 
To  achieve  this  it  is  necessary  reduce  the  SLOWC  program 
parameter  to  cause  smaller  adjustments  to  the  trial  solution. 
This  indicates  that  for  small  difference  in  boundary  B  values, 
the  program  is  sensitive  to  small  changes.  If  we  exaggerate 
the  distance  the  disk  must  roll  between  these  two  points,  the 
reason  for  this  behavior  becomes  evident.  (Fig.  12)  Here  we 
specified  that  the  disk  roll  5  times.  The  initial  B  value  is 
45  degrees.  If  we  req-aire  that  the  disk  make  a  3.6  x  10'® 
degree  turn  to  the  left  (1  X  10‘^°  rotation)  we  obtain  one 
optimal  solution.  However,  if  we  require  that  the  disk  make 
a  3.6  X  10'®  degree  turn  to  the  right  (-1  X  10’^®  rotation)  we 
obtain  a  much  different  solution.  Hence,  for  very  small 
changes  in  angle  the  solution  varies  widely.  If  we  specify  a 
zero  rotation,  there  is  no  clear  preference  for  the  most 
optimal  solution,  and  the  program  cannot  converge.  The  same 
holds  true  as  we  approach  a  perfectly  straight  line  path.  We 
specify  the  initial  and  final  position  and  the  initial  and 
final  (p  values,  which  theoretically  are  the  same.  However 
numerically,  there  is  a  small  difference.  This  difference  is 
sufficient  to  induce  the  problems  above  and  prevent 
convergence.  Fortunately,  an  analytic  solution  to  the  exact 
straight  line  problem  is  easily  obtained. 
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The  computer  program  used  includes  several  control 
parameters  which  assist  in  finding  a  solution.  While  running 
various  simulations,  the  following  trends  were  noted: 

•  Slowing  down  the  convergence  by  decreasing  SLOWC  can 
help  find  a  solution  when  the  maximum  error  fluctuates  near 
some  minimum  value.  However,  doing  so  does  not  guarantee  a 
solution. 

•  The  SCALV  values  should  represent  the  absolute  magnitude 
of  a  typical  solution  value.  Where  this  value  is  not  known, 
use  a  small  SCALV  to  start. 

•  The  trend  of  the  maximtim  error  with  iterations  should  be 
used  as  a  guide  as  to  whether  the  program  will  converge  to  a 
solution.  However,  the  trend  neither  guarantees  nor  excludes 
convergence . 

As  demonstrated,  the  rolling  disk  problem  requires  a  path 
which  is  continuous  and  smooth.  And  since  we  specify  the 
distance  the  disk  must  roll  and  number  of  turns  the  disk  must 
make,  the  solution  is  only  optimal  for  those  specifications. 
In  the  more  general  mobile  robot  problem,  we  look  for  the  most 
optimal  path  which  need  only  meet  the  initial  and  final 
boundary  conditions. 
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III.  OPTIMAL  CONTROL  OF  A  MOBILE  ROBOT 


In  Chapter  II  we  developed  a  two  point  boundary  value 
problem  by  applying  Pontryagin's  Minimization  Principle  to  the 
equations  of  motion  for  a  rolling  disk.  We  then  solved  the 
resulting  two  point  boundary  value  problem  by  a  numerical 
relaxation  technique.  Having  demonstrated  that  the  process 
above  provides  an  optimal  solution  for  a  simple  nonholonomic 
system,  we  return  to  the  more  difficult  mobile  robot  problem. 
Our  goal  is  to  move  the  robot  from  a  initial  position  and 
orientation  to  a  desired  one  within  a  fixed  amount  of  time,  in 
an  optimal  manner,  and  using  feedback  control.  Before 
developing  our  solution,  however,  we  first  look  at  some 
conventional  theory  regarding  mobile  robot  control.  This  is 
necessary  to  motivate  the  approach  used  to  solve  our  problem. 

A.  LITERATURE  SURVEY 

Extensive  research  into  non-linear  control  design  of  two 
wheeled  mobile  robots  exists.  For  the  problems  of  path 
following  and  tracking,  relatively  classical  non-linear 
control  techniques  have  been  applied  successfully.  [Refs.  2,3] 
However,  the  problem  of  stabilization  about  a  point  is  more 
difficult.  Brockett's  Theorem  [Ref.  0]  shows  that  smooth  non¬ 
time  varying  control  laws  cannot  be  developed  for  such 
problems . 
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This  is  the  case  for  all  driftless,  nonholonomic  systems  of 
the  form 


X  =  [K]  a 


(46) 


Using  classical  Lyapunov  analysis,  Reference  1  presents  a 
general  method  for  finding  time  varying  control  laws  for 
driftless  systems.  In  Reference  6  and  7,  the  authors  develop 
smooth,  time  varying  and  piecewise  continuous  control  laws. 
While  these  controls  employ  closed  loop  feedback,  none 
considers  the  optimality  of  the  solution.  In  this  research, 
we  apply  optimal  control  theory  to  the  mobile  robot  problem. 


B.  STATE  AND  COSTATE  EQUATIONS  FOR  THE  MOBILE  ROBOT 

In  our  approach  to  the  mobile  robot  control  problem  we 
first  move  the  robot  onto  the  line  described  by  the  final 
position  and  orientation  of  the  robot.  (Fig.  13)  The  robot 
may  then  roll  directly  to  the  final  desired  position.  The 
point  at  which  the  robot  will  intersect  the  line  and  the 
manner  in  which  the  robot  will  approach  the  line  is  not 
specified.  The  goal  of  the  optimal  control  problem  is  to 
minimize  the  distance  between  the  desired  position  of  the 
robot  and  the  point  of  intersection  of  the  robot  with  the 
line,  in  a  way  that  utilizes  the  minimum  amount  of  energy. 
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1.  Basic  Kinematic  Relationships 

Returning  to  the  coordinate  and  velocity  descriptions 
of  Figure  1,  we  begin  with  the  kinematic  equations, 

X  =  U,  COS0 

y  =  ursine 
e  = 

From  the  desired  final  conditions  of  X^,  Y^,  and  6^,  we 

redefine  our  states  in  terms  of  the  difference  between  the 
final  condition  and  the  current  coordinate  value  such  that 

Ax  =  X^  -  X 

AY  =  Y^  -  Y  (48) 

Ae  =  -  0 

As  our  approach  suggests,  we  require  that  the 
difference  between  the  robot  angle  and  desired  angle  be 
minimized  or, 

A0  =  0  (49) 

We  also  require  that  the  perpendicular  distance  between  the 
robot  and  the  line  be  minimized.  This  distance  can  be  defined 
in  trigonometric  terms  as 

p  =  {Ay  cosQ^  -  AX  sin0j)  (50) 

In  order  to  converge  p  and  AO  to  zero  asymptotically, 
we  find  that  the  second  input  should  be  a  function  of  the 
first  input.  Our  analysis  is  based  on  the  application  of 
Lyapunov's  Stability  Theorem. 
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2 .  Application  of  Lyapunov' s  Theorem 

Lyapunov's  Theorem  of  asymptotic  stability  provides 
that  the  equilibriiim  of  zero  for  a  system, 


x=f(x,  u) 


(51) 


is  asymptotically  stable  if  there  exists  a  positive  definite 
function  such  that  the  first  derivative  of  that  function  is 
non- increasing.  [Ref.  9]  In  our  case  we  define  a  Lyapunov 
function  as 

V  =  |(p2  +  Ae2)  (52) 
The  first  derivative  of  this  function  is, 


where 


V  =  -p(cos0^  sin6  -  sinSj,  cos6) 
=  pC/iSin(A0)  -  AQU2 

« -Ae(o,  -  pu.  ) 

=  -Ae  (U;  -  pU,f(A0)  ) 

f(A6)  4 


If  we  choose 

£4  =  p£/i  f{Ae)  +  a  Ad 
We  may  express  equation  (53)  as 


V  =  -a  Ae^ 


(53) 


(54) 


(55) 


(56) 


which  is  negative  semidef inite . 
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This  equation  satisfies  Lyapunov's  Theorem  for  all  a  greater 
than  zero,  provided  that  A0  is  not  equal  to  zero.  In  the 
event  Ad  is  equal  to  zero,  the  derivative  of  the  Lyapunov 
function  becomes  negative  semidefinite  and  the  asymptotic 
stability  can  be  guaranteed  by  applying  the  theorem  by 
Mukherjee  and  Chen  [Ref.  10]. 

The  choice  of  the  second  control,  given  by 

equation  (55),  in  terms  of  the  first  control,  U^,  and  state 
feedback  leaves  us  with  the  task  for  the  design  of  one  input 
for  the  system,  namely  17^.  will  be  designed  using  optimal 
control  methods.  The  gain,  a,  affects  the  rotational  motion, 
and  from  Lyapunov's  Theorem,  a  must  be  greater  than  zero  at 
all  times.  Various  schemes  have  been  tested  to  determine  the 
best  use  of  this  parameter  in  an  optimal  solution  for 
3.  Variations  of  tha  Robot  Problam 

Since  the  only  requirement  of  U2  is  that  equation  (53) 
be  negative  definite,  there  are  infinite  variations  of  this 
function  which  we  could  employ.  In  the  sections  below,  we 
produce  five  possible  variations  and  discuss  the  application 
of  optimal  control  to  each  of  them. 

a.  Robot  1,  virtual  Robot  Problem 

In  this  approach,  in  addition  to  the  original 
robot,  we  define  a  virtual  robot  which  may  travel  only  on  the 
line  of  the  desired  angle.  (Fig.  14)  This  approach  is 
somewhat  similar  to  the  bi-directional  approach.  [Ref.  11]  The 
virtual  robot  may  roll  forward  or  backward,  but  not  turn. 
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Our  goal  is  to  have  the  two  robots  meet  at  some  unspecified 
point  on  the  line.  This  allows  a  smooth  trajectory  for  each 
robot.  Furthermore,  this  positions  and  orients  the  real  robot 
in  line  with  the  desired  final  location,  requiring  only  a 
trivial  solution  to  complete.  Fortunately,  the  impact  of  this 
addition  to  the  kinematic  equations  is  minimal .  Defining  the 


position  of  the  virtual  robot  as  and  Y^,  which  are  now 


variables, 


the  difference  between  the  two  positions  is 

=  (X^-X) 

Ay  =  (Y^-Y) 


(57) 


The  difference  between  the  orientation  of  the  two  robots  is 


A0  =  (0^  -  0) 


(58) 


where  6^  is  the  constant  desired  angle  of  orientation.  The 


differential  equations  of  motion  now  take  the  form. 


cos0jl7^  -  cos{0j  -  A0)  l/j 
sinQ^U^  -  sin(0j  -  A0) 
-aA0  -  pC/^f  (A0) 


(59) 


Where  is  the  forward/backward  velocity  of  the  virtual 
robot.  Applying  the  seune  optimal  control  theory  as  before,  we 
define  our  cost  function  as 

jr  =  ^  (AX^  +  Ay2  +  A0^)  c  +  +  Ud) 

2  f  J  2> 

^0 


The  terminal  cost  gives  a  penalty  for  not  going  to  the  desired 
final  condition,  AX,  AY,  and  A6 .  C  is  the  weighting  parameter 
for  this  cost.  This  cost  is  necessary  as  the  values  of  the 
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final  states  tend  to  float  and  hence  we  cannot  assume  as 
before  that 

()X{tf)=0  (61) 

From  equation  (9)  we  see  that  to  satisfy  the  necessary 
conditions  for  optimal  control,  we  need 

at  t=t.  (62) 

ax 

where 

4)  =  -i  (AX2  +  Ay2  +  Ae^)  (63) 

This  implies  ti^at  the  constraints  at  the  final  times  are 

(Xj)  =  C(Ay)  (64) 

Ik3  we  shall  see  later,  this  is  important  in  minimizing  the 
error  at  the  final  time.  From  the  definition  of  the 
Hamiltonian, 

H  =  L  +  X'^f  (65) 

where 

L=X{ul  +  U^)  (66) 

and  f  is  the  right  hand  side  of  equation  (59)  .  We  apply 
equation  (12)  for  both  and  U^.  From  this  we  can  show  that 
the  optimal  control  inputs  are, 

U^=X^cos  (d^  -  A0)  +  (6^  -  AO)  +  X3P^(Ae)  (67) 

and 

=  -X^cosQ^  -  X2sin6j^  (68) 
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Applying  equations  (10)  and  (11)  we  can  develop  a  full  set  of 
equations  for  our  two  point  boundary  value  problem. 


cosQ^U^  -  cos(Q^  -  A0) 

(A.y) 

sinOjC/j  -  sin(0j  -  A0) 

(aV) 

-aA9  -  pUj^f(Ad) 

(A0) 

.= 

-I.3  t/jSinO^f  (A0) 

A,3  C/^cosOjjf  ( A0) 

K 

X^Uj^siniQ^  -  A0) 

X 

-  ^2  (/iCos  (0^-AO) 

^3 

+  X3  (a  +  pU^  f^(A0)  ) 

(69) 


where  and  p,  are  as  described  above.  The  function 

f  (AO )  requires  special  handling  due  to  the  AO  term  in  the 
denominator.  By  L'Hopital's  rule  we  know 


lim  _  lim  sin(Ae)  _  , 

Ae-0  ■  Ae-0  — Ae - ^ 


(70) 


and 


lim  _  lim  cos(A0)  sin(A6) 

A0-O  ^  -  A0-O  - A0 -  ■  - - ° 


A02 


Therefore,  to  maintain  continuity  during  numeric  processing  we 
define 


/(A0)  = 


,  for  Ae.o 
A0 


1, 


for  A0=O 


(72) 


and 


f'(A0) 


cos  (A0) 
A0 

0, 


sin  (AO) 
A02 


for  AB*0 
for  A0=O 


(73) 


From  equation  (69)  the  utility  of  a  numeric  solution  to  the 
two  point  boundary  value  problem  becomes  clear. 
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The  process  for  setting  up  the  expressions 

for  the  computer  program  is  similar  to  the  rolling  disk 
problem,  although  more  lengthy  and  involved.  The  final 
expressions  can  be  found  in  the  DIFEQ.FOR  subroutine  of 
Appendix  B. 

Upon  testing  the  virtual  robot  problem,  it  became 
obvious  that  the  discontinuous  path  was  not  a  problem  for  the 
program.  (Fig  15)  A  look  at  the  velocity  components  explains 
why.  (Fig  16)  When  broken  into  components  the  velocities  are 
smooth.  We  also  note  that  the  non- zero  velocities  at  the 
initial  and  final  time  are  not  a  source  of  concern,  as  this  is 
a  kinematics  problem.  Furthermore,  since  this  is  a  motion 
planning  problem  and  not  feedback  control,  a  two  robot  model 
has  no  practical  disadvantages.  However,  the  next  robot  models 
we  consider  are  based  upon  a  single  robot. 

b.  Robot  2,  Single  Mobile  Robot 

The  kinematic  equations  of  motion  for  this  problem 
are  similar  to  the  equations  for  the  two  mobile  robot  except 
that  and  are  fixed.  As  a  result, 

and  the  kinematic  equations  appear  as 

-cos  (0j  -  A0) 

-sin(0j  -  A0)  (75) 

-a  A0  -  pU^f  ( A0) 
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We  define  the  cost  function  for  this  problem  as 

J"  =  I  (AJST^  +  +  ^02)  +  jl(Ul)  (76) 

Co 

which  has  the  same  terminal  costs  as  the  two  robot  problem. 

We  again  use  the  definition  of  the  Hamiltonian 

H  =  L  *  k'^f  (77) 

where 

L=k(ul)  (78) 

and  f  is  the  right  hand  side  of  equation  (75)  .  Applying 
equation  (12)  for  CJj  we  can  show  that  for  optimal  control, 

^=A,3lCos(0j  -  A0)  +  A2sin(0j  -  A0)  +  k^pf(^Q)  (79) 

Applying  equations  (10)  and  (11)  we  again  develop  the 
equations  for  our  two  point  boundary  value  problem. 


-cos  (0^  -  A0) 

(AX) 

-sin(0^  -  AB) 

(aV) 

-aA0  -  pC7iJf(A0) 

(A0) 

-A3  U^sinB^f  (^B) 

K 

Aj  Uj^cosB^f  i^B) 

K 

Ai(7iSin(0^  -  A0) 

-  A2  U3C0S  (0J-A0) 

''■3 

+  A3  (a  +  pU^  jf'(A0) ) 

which  is  the  same  as  equation  (69)  except  for  the  first  two 
differential  equations.  The  resulting  expressions  can  be  seen 
in  the  DIFEQ.FOR  subroutine  of  Appendix  C; 
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As  expected,  the  problem  works  adequately  for 
apparently  non-smooth  paths.  (Pig.  17,  18)  However,  for  the 
case  where  we  ask  the  robot  to  change  only  its  angle  of 
orientation,  the  solution  given  indicates  that  the  robot  only 
spins  without  moving  forward.  While  this  solution  is  indeed 
optimal,  it  is  evident  from  the  definition  of  angular  velocity 
in  equation  (80)  that  the  robot  turns  without  moving, 
c.  Robot  3,  Contrained  Robot  Model 

To  contrain  the  angular  rotation  to  prevent 
rotation  when  the  robot  is  stopped,  we  must  ensure  that  U2  is 
entirely  a  function  of  U^.  In  order  to  meet  Lyapunov's 
Theorem  we  must  ensure  that  equation  (53)  is  negative  at  all 
times.  In  order  to  meet  both  requirements,  we  chose  [7^  of  the 
form 

C/j  =  pU^fi^B)  *  ug{U^)liB  (81) 


where 


forUj^^O 
for  1/2^0 


(82) 


This  expression  guarantees  that  Lyapunov's  Theorem  is 
satisfied.  Application  of  equation  (12)  results  in  the 
control , 

=  AiCos(0j  -  A0)  +  l3Pf(A0)  (83) 


which  is  the  same  control  from  Robot  2. 
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Applying  the  other  necessary  conditions  for  optimal  control 
gives  the  equations  for  the  two  point  boundary  value  problem. 


-cos  (6j  -  A0) 

(AX) 

-sin(6rf  -  A0) 

(Ay) 

-aABg(Uj^)  -  pU^f(AQ) 

(AQ) 

-X3  sin  0^  f  ( A0 ) 

K 

X3  Uj^cosQ^f  (Ad) 

K 

Xj^U^sin(Q^  -  A0) 

-  k^Uj^cos  (0^-A0) 

''■3 

+  k^(ag(U^)  +pU^f'(Ae)) 

The  resulting  expressions,  obtained  as  before,  can  be  seen  in 
the  DIFEQ.FOR  subroutine  in  Appendix  D. 

The  solutions  for  this  new  variation  are,  in  most 
cases,  the  same  as  those  from  Robot  2.  (Fig.  19)  The  most 
significant  difference  is  for  the  case  where  we  ask  the  robot 
to  change  only  its  angle  of  orientation.  The  solution  is  now 
a  trivial  one;  the  robot  does  not  move.  (Fig.  20)  When  AX  and 
AY  are  zero,  the  quantity  p  is  zero  and  thus. 


U^=k^Cos(e^  -  A0) 

+  k^SinO^j  - 

A0) 

(85) 

And 

if 

and  X2  are 

zero  for 

all 

time,  then 

=  0 

(86) 

9{U^)  =  0 
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and  therefore, 


0 


= 

g(u^)  =  0 
=  0 
aV  =  0 

(87) 

A0  =  0 

-  0 

X2  =  0 

^3  =  0 

The  terminal  costs  are  met  since 

(88) 

where  A6  is  fixed  and  the  X3  term  simply  becomes  a  large 
enough  constant  to  meet  this  constraint  (Fig.  21)  . 
Heuristically,  this  says  that  the  most  efficient  manner  to 
achieve  the  desired  final  condition  is  not  to  go.  Such 
results  occur  any  time  two  of  the  three  states,  AX,  AY  or  Ad, 
are  equal  to  zero.  In  such  cases  where  p  or  A6  is  zero  at  all 
times  and  C7j  becomes  zero,  the  state  equations  of  motions  from 
equation  (84)  are  all  equal  zero  and  give  a  trivial  solution. 

So  far,  we  have  chosen  the  value  of  a  at  the 
outset  of  the  program.  However,  as  we  shall  show  later,  a  has 
a  direct  impact  on  the  final  solution. 
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d.  Robot  4,  Robot  3  with  Higb/Low  a  Control 

Defining  a  as  a  control  is  complicated  by  the  fact 
that  a  must  be  positive  to  satisfy  Lyapunov's  Theorem.  We 
therefore  define  the  cost  function 

j  =  j  +  a)  (89) 

^0 

where  a  is  greater  than  zero  for  all  t.  The  resulting 
Hamiltonian  is 

H  =  -  U?  +  a 

-k^cosiQ^  -  A6)  (90) 

-Xj sin  (6jj  -  A6) 

-X3  (pC/^f  (AS)  +  agiU^)  ^6) 

Applying  equation  (12)  to  CTj  we  find  the  same  result  as 
before, 

C/3  =  X^cosOj,  -  A0)  +  X2sin(6j^  -  A0)  +  (91) 

For  the  second  control  we  consider  only  those  terms  in  the 

Hamiltonian  associated  with  a. 

H^=a  (1  -  X^g(U^)  A0)  (92) 


In  order  to  minimize  the  Hamiltonian  and  maintain  a  positive 
0!  we  define 


where 


a 


t 


min' 

mcix' 


for  P  >  ol 
for  P^OJ 


(93) 


P  =  1  -  X^g(U,)  A0  (94) 
The  resulting  equations  for  the  two  point  boundary  value 

problem  are  the  same  as  equation  (84) ,  except  that  the  value 
of  Of  depends  on  /3.  The  resulting  S  expressions  are  listed  in 
the  DIFEQ.FOR  program  in  Appendix  E. 
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Using  this  variation  of  U2,  the  progrcim  has 
difficulty  converging  in  many  cases.  The  non-linear  nature  of 
a  is  the  source  of  this  difficulty.  (Fig.  22)  In  many  cases, 
the  converged  solution  is  the  same  as  Robot  3.  Since  there 
appears  to  be  little  advantage  to  this  variation,  we  seek  a 
more  proportional  a  control . 

e.  Robot  5,  Robot  3  with  Proportional  a  Control 

To  develop  a  proportional  a  control,  we  start  with 
a  new  cost  function, 

j  =  J'l(u^  +  a^)  (95) 

Cq 

We  find  that  Uj  is  the  same  as  before.  If  we  only  consider 
the  a  terms  then, 

2H,  =  H  =  -  2X3agr(C7i)  A0  (96) 

However,  since  gdJ^)  equals  zero  for  equal  to  zero,  is 
already  a  minimum  when  equals  zero  and  a  approaches  the 
positive  side  of  zero.  Thus  we  only  need  consider  the  case 
where  is  not  zero.  In  this  case  gdJ^)  is  one  and  will  be 
dropped  in  the  remaining  expressions.  Factoring  we  find, 

if.  =  (a  -  -  (XjAe)^  (97) 


If  we  neglect  the  second  part  of  this  expression  as  it  is  not 
a  function  of  a,  then  to  minimize  H„, 


a  = 


X,A6,  for  X,A0>O 


a 


min ' 


for  X3A05O 


(98) 


where  some  value  greater  than  zero. 
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The  resulting  differential  equations  differ  from 
equation  (84)  only  in  that  alpha  is  now  a  function  of  X3  and 
A6 .  This  must  be  taken  into  account  when  developing  the  S 
expressions.  The  changes  to  the  resulting  S  expressions  can 
be  seen  in  the  DIFEQ.FOR  subroutine  in  Appendix  F. 

In  general,  this  variation  gives  better  solutions 
than  all  other  variations  discussed.  The  proportional  alpha 
control  is  more  likely  to  converge  and  gives  an  apparently 
more  optimal  solution.  It's  tendency  to  converge  is  more  well 
behaved  than  other  variations.  However,  it  still  requires  a 
certain  amount  of  user  interaction  to  set  the  value  of  amin 
and  other  progreun  parameters  to  a  value  which  will  achieve 
convergence.  Trends  and  comparisons  of  this  and  other 
variations  of  the  mobile  robot  problem  is  the  subject  of  the 
following  section. 

C.  DISCUSSION  OF  RESXTLTS 

There  are  many  factors  which  affect  convergence, 
optimality  and  error  of  the  final  solution.  Since  each 
variation  was  designed  with  a  slightly  different  intent, 
comparison  is  difficult.  This  section  discusses  general 
trends  noted  during  extensive  testing  of  the  programs. 

The  state  variable  solutions  to  the  two  point  boundary 
value  problems  are  in  terms  of  the  difference  between  the 
current  and  desired  value.  For  presentation,  we  convert  these 
values  into  X,  Y,  and  6  coordinates . 
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In  the  case  of  Robot  1  desired  coordinates  move. 


Therefore,  we  substitute  our  solution  back  into  the 
differential  equations  to  obtain  velocity  profiles.  For  Robot 
1  only,  we  then  use  a  crude  trapezoidal  integration  to 
determine  the  values  of  X,  Y,  6,  and  at  each  time. 
There  is  a  small  error  imposed  by  this  integration  which  may 
appear  in  the  path  plots  for  Robot  1.  The  true  final  error 
all  cases  is  taken  directly  from  the  solution  values  of  AX, 
AY,  and  A6  and  is  not  affected  by  this  integration.  By  true 
final  error  we  refer  to  the  difference  between  the  final  robot 
coordinates  and  the  desired  robot  coordinates.  For  most  cases 
this  true  final  error  can  be  made  negligibly  small  by 
adjusting  a  and  C. 

For  the  other  Robot  variations  the  velocity  profiles  are 
obtained  in  the  same  way.  However,  since  X^  and  Y^  are  fixed 
for  these  problems,  determining  the  X,  Y,  and  0  coordinates  is 
simply  a  matter  of  subtraction.  The  final  error  shown  in 
these  problems  is  more  representative  of  the  true  final  error. 

For  the  sake  of  comparison,  the  results  of  each  variation 
was  tested  against  a  single  pseudo- energy  cost  function. 

J  =  dt  (99) 

Co 

This  cost  value  has  mixed  units.  For  simplicty  we  consider 
the  costs  shown  in  the  figures  below  in  nondimensional  units. 

Length^  (100) 

Time 
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While  Robot  5  was  the  only  variation  developed  from  this 
performance  parameter,  this  is  the  most  encompassing  cost 
description.  The  terminal  costs  were  not  considered  for  this 
part  as  these  are  compared  in  the  form  of  final  error. 

1.  Effect  of  Varying  ex  and  C  Parameters 

For  each  variation  of  the  Robot  program,  the  effect  of 
varying  the  rotational  gain,  a,  and  terminal  cost  weighting, 
C,  is  different.  Rather  than  present  all  the  possible 
variations  here,  some  of  the  more  significant  trends  are 
sampled. 

The  variation  of  a,  strongly  affects  the  ability  of 
the  program  to  converge  as  well  as  the  optimality  and  error  in 
the  final  solution.  There  is  a  range  of  a  for  which  each 
program  will  converge  for  a  given  set  of  boundary  conditions. 
A  typical  example  of  the  effect  of  varying  a  can  be  seen  in 
Figure  23 .  These  are  the  paths  given  by  Robot  1  for  boundary 
conditions  of 

Xo  =  0  Xf  =  10 

Yq  =  0  Yf  =  10  (101) 

Og  =0"  0^  =  90° 

We  must  also  look  at  the  angular  trajectory  for  these 
solutions.  (Fig.  24)  Note  that  for  the  extreme  values  of  a, 
there  is  a  larger  error  in  the  final  solution.  Also  note  that 
the  paths  are  of  different  lengths,  indicating  that  some  a's 
give  more  optimal  solutions  than  others.  The  energy  cost  plot 
(Fig.  25)  shows  the  effect  of  a  on  this  cost. 
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From  Figures  23  and  25,  an  a  value  of  25  appears  to  give  both 
a  minimum  final  error  and  cost.  However,  the  results  for 
this  program  and  set  of  boundary  conditions  cannot  be  used  as 
a  guideline  for  all  programs  or  cases. 

In  the  particular  case  of  Robot  4,  the  use  of  and 

must  be  handled  carefully.  If  the  two  values  are  greatly 
different,  the  program  will  have  difficulty  converging.  If 
the  two  values  are  too  close,  there  is  no  advantage  to  using 
this  program. 

Robot  5  tends  to  converge  for  a  larger  range  of  a 
values.  In  general,  any  a  for  which  the  other  programs 
converge  will  usually  work  for  Robot  5.  However  in  many 
cases.  Robot  5  allows  a  lower  a,  and  a  lower  final  energy 
cost.  As  a  rule  of  thumb,  the  lowest  for  which  the 

program  converges  gives  the  most  optimal  solution. 

The  variation  of  C  is  more  straightforward;  the  higher 
the  value  of  C,  the  smaller  the  final  error.  (Figs.  26,  27) 
However,  certain  limits  do  apply  to  this  guideline.  If  C  is 
too  high  for  a  given  set  of  boundary  conditions,  the  program 
tends  not  to  converge.  There  is  also  a  price  for  this 
accuracy.  (Fig.  28)  In  general  a  more  accurate  final 
solutions  show  a  higher  final  cost. 
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2.  San^le  of  Test  Cases 

The  Robot  programs  have  been  tested  for  many  different 
boundary  conditions  and,  subject  to  user  supplied  parameters, 
give  optimal  solutions.  The  following  six  cases  are 
representative  of  the  results.  For  each  program  variation  we 
provide  the  best  known  control  parameters  for  that  program  and 
set  of  boundary  conditions.  In  this  way  we  compare  the  best 
result  for  each. 

a.  90  Degree  Turn  Problem 

For  this  case  the  boundary  conditions  are: 

Xq  =  0  =  10 

Yq  =  0  Yf  =  10  (102) 

00  =0“  Sf  =  90° 

Most  of  the  programs  give  a  similar  result  for  this  problem, 
except  Robot  1.  (Fig.  29,  30)  This  problem  requires  a 

relatively  high  a  to  achieve  convergence.  As  a  result,  the 
programs  which  use  o;  as  a  control.  Robot  4  &  5,  show  no 
advantage.  (Fig.  31,  32)  Although  the  Robot  l  solution  takes 
a  longer  path,  by  our  definition  of  cost,  its  solution  is  more 
optimal . 

Jb-  30  Degree  Angle  Parking  Problem 

For  this  case  the  boundary  conditions  are: 

=  0  Xf  =  0 

To  =  0  Yf  =  2  (103) 

00  =0°  0f  =  30° 

Here,  the  effect  of  a  control  is  more  evident.  (Fig.  33,  34) 
In  the  case  of  Robot  4,  the  program  has  a  more  difficult  time 
converging  because  of  the  non-linear  a.  As  a  result,  its 


44 


solution  is  the  least  optimal.  (Fig.  35)  Robot  5,  however, 
gives  the  best  optimal  solution.  While  Robot  5's  final  error 
is  higher  than  some  others,  considering  the  order  of  magnitude 
of  the  final  error,  the  difference  is  negligible. 

c.  270  Degree  Turn  Problem 

This  case  is  inherently  not  optimal.  The  angular 
displacement  required  could  be  achieved  more  easily  by  going 
to  -90°  instead.  However,  these  boundary  conditions  provide 
a  more  demanding  test. 

Xq  =  0  Xf  =  0 

Yo  =  0  Yf  =  2  (104) 

00  =  0“  0^  =  27  0° 

The  programs  overcome  the  angular  displacement  problem  by 
stopping  and  backing  part  way  though  the  maneuver.  (Fig.  36, 
37,  38)  Based  on  the  final  error  and  energy  cost,  no  program 
has  any  distinct  advantage  over  the  others  for  this  maneuver. 
(Fig.  39) 

d.  180  Degree  Turn  On  A  Point 

For  all  Robot  programs  using  partial  feedback,  if 
two  of  the  three  state  variables,  AX,  AY  or  A0 ,  have  zero 
difference  between  their  initial  and  final  boundary 
conditions,  the  result  will  be  the  trivial,  "don't  go" 
solution.  If,  however,  there  is  at  least  small  difference 
between  the  initial  and  final  boundary  conditions  for  two  of 
the  three  states,  a  non- trivial  solution  can  be  obtained.  For 
this  reason,  we  use  a  small  difference  between  the  initial  and 
final  X  boundary  conditions  for  this  problem. 
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(105) 


Xq  =  0  =  -0.01 

Kq  =  0  =  0 

00  =  0^*  6;  =  180° 

Robot  1  and  2,  which  do  not  include  partial  feedback,  simply 
turn  and  go  to  the  desired  position.  (Fig.  40)  The  remaining 
solutions  are  all  similar.  For  all  cases  the  energy  cost  is 
the  same,  with  similar  final  error  for  the  feedback  problems. 
(Fig.  41) 

e.  Parallel  Parking  Problem 

This  case  proved  the  most  difficult  of  any  for  the 
program  to  solve.  Again,  we  must  avoid  the  boundary 
conditions  where  two  of  the  three  boundary  conditions  have 
zero  difference.  Only  Robot  5  was  able  to  produce  a  solution 
with  reasonable  minimum  error.  (Fig.  42,  43,  44)  This  is 
because  only  Robot  5  supports  proportional  a  control .  The 
choice  of  a  is  critical  to  the  resulting  solution.  Since  a  is 
a  gain  which  affects  the  angular  velocity,  too  high  an 
results  in  highly  non-linear  solutions.  This  is  true  even  in 
the  case  of  Robot  5.  (Fig  45,  46,  47)  Nevertheless,  a 

judicious  choice  of  CK^in  results  in  an  optimal  and  low  final 
error  solution. 
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f.  Trivial  Straight  Line  Case 


For  the  trivial  case  where  we  ask  the  robot  to 
move  along  a  straight  line  from  one  position  to  another,  the 
solution  converges  very  quickly  to  the  obvious  solution.  A 
few  iterations  are  necessary  to  bring  the  costate  variables  to 
their  proper  values.  These  programs  have  no  problem 
converging,  unlike  the  disk  problem,  because  there  are  no 
competing  boundary  conditions. 

3.  Other  Trends  Noted 

The  effect  on  of  the  initial  guess  cannot  be 
overstated.  For  each  case,  the  initial  guess  was  based  on  the 
straight  line  path  between  the  initial  and  final  points. 
(Fig.  48)  The  costates  were  assumed  to  be  some  small,  non¬ 
zero  value.  The  Robot  programs  show  strong  tendency  towards 
convergence  even  when  given  such  a  crude  guess. 

In  some  cases,  particularly  highly  oscillatory 
solutions  aoout  small  values,  the  program  will  tend  toward 
convergence  but  then  hover  at  some  error  value,  or  oscillate 
between  two  small  error  values.  In  these  cases  the  best 
response  is  to  adjust  the  SLOWC  parameter  to  slow  the  program 
convergence.  This  causes  the  program  to  make  smaller 
corrections  where  it  might  be  jumping,  back  and  forth,  over  a 
solution.  Doing  this  does  not  guarantee  a  solution,  but  it  is 
helpful  in  some  cases. 
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The  Program  uses  an  EPSILON  variable  to  determine  the 
value  of  f(Ad)  when  Ad  is  nearly  equal  to  zero.  If  A6  is  less 
than  EPSILON,  we  consider  it  sufficiently  rlode  to  zero  to  use 
the  definition  of  f(Ad)  equal  to  one  and  f'  (A8)  equal  to  zero. 
From  experimenting  with  the  programs,  any  reasonedDly  small 
value  for  this  varicdDle  will  give  the  same  solution.  This  is 
true  as  A6  rarely  approaches  zero  within  a  solution,  except  at 
the  final  time.  At  the  final  time,  the  con^uter  determines 
the  values  of  the  expressions  based  on  the  final  boundary 
condition  expressions.  Since  the  function  f{A6)  does  not 
appear  in  these  expressions,  the  value  of  EPSILON  has  no 
effect.  In  the  case  where  Ad  is  less  than  EPSILON  at  some 
other  time,  the  impact  appears  negligible  for  all  reasonable 
values  of  EPSILON. 

The  contrained  robot  problems  use  the  control  of 
equation  (82) .  The  program  uses  the  variable  EPSIL0N2  to 
determine  if  the  value  of  is  sufficiently  close  to  zero  for 
3r(Ui)  to  be  equal  to  zero.  The  value  used  for  this  EPSIL0N2 
has  been  found  to  make  little  difference  in  the  solution  as 
is  rarely  zero  for  any  length  of  time.  (Fig.  49)  Where  the 
value  of  U]^  is  less  than  EPSIL0N2  at  some  time,  the  solution 
profile  tends  to  be  non-smooth,  making  it  difficult  (though 
not  impossible)  for  the  program  to  converge.  On  the  next 
iteration,  the  time  location  of  the  zero  may  be  moved.  As 
a  result,  the  program  solution  tends  to  place  the  exactly  zero 
CZj  velocities  between  the  discrete  time  points. 
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Hence,  for  reasonably  sized  values  of  EPSIL0N2,  the  solution 
is  not  affected  significantly.  However,  for  convergence  sake, 
the  value  of  EPSIL0N2  should  be  sufficiently  small,  1  x  10''^ 
or  less. 
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IV.  SUMMARY  AMD  RECOMMENDATIONS 


In  this  thesis,  we  have  demonstrated  a  method  for  finding 
an  optimal,  open  loop,  timr  /arying  control  for  a  nonholonomic 
system.  In  general  this  method  employs  Pontryagin's 
Minimization  Principle  to  find  the  state  and  costate  equations 
for  an  optimal  control.  Then  a  numerical  relaxation  technique 
is  applied  to  the  resulting  two  point  boundary  value  problem. 
For  the  specific  problem  of  a  two  wheeled  mobile  robot,  we 
first  develop  a  partial  feedback  law  using  Lyapunov's  Theorem. 
In  doing  so,  we  create  a  system  which  does  not  fall  under 
Brockett's  Theorem  and  thus  has  an  equilibrium  point  solution. 
The  method  has  been  found  to  give  optimal  solutions  for  all 
cases  of  the  mobile  robot  problem.  However,  in  the  case  where 
two  of  the  three  state  boundary  conditions  are  exactly  the 
same  at  the  initial  and  final  time,  the  optimal  solution 
obtained  is  a  trivial  one.  The  optimality  of  the  solution  is 
subject  to  the  definition  of  the  cost  function,  the  weight  of 
the  terminal  cost,  and  choice  of  rotational  gain,  a.  While 
closed  loop  controls  of  mobile  robots  are  obtainable,  they  are 
not  optimal  in  any  sense.  For  an  application  where  efficiency 
is  important,  the  method  demonstrated  here  would  be 
advantageous . 
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The  results  obtained  opens  I’p  a  number  of  areas  for 
further  research  into  this  and  related  problems: 

•  Refining  the  angular  feedback  g(Uj^}  such  that  the 
control  is  more  proportional  to  would  result  in  a  more 
smooth  control . 

•  A  more  refined  algorithm  for  creating  the  initial  guess 
of  states  and  costates  would  help  ensure  convergence  and  allow 
more  freedom  in  choosing  problem  parameters  C  and  ot.  The 
initial  guesses  could  be  based  on  known  solutions  to  some 
commonly  used  boundary  conditions. 

•  The  method  described  here  requires  that  the  final  time 
be  fixed.  A  more  general  solution  to  the  free  end  time,  or 
minimum  time  problem  is  desirable. 

•  This  method  could  be  used  for  in  line  path  planning  of 
a  mobile  robot.  By  using  the  last  solution  as  the  new  initial 
guess,  the  program  could  constantly  update  the  path  for  the 
robot  to  follow.  As  the  last  solution  would  be  a  very  good 
guess,  the  program  would  converge  very  quickly. 

•  The  mobile  robot  problem  presented  is  a  kinematics 
problem.  A  more  realistic  second  order  problem  would  consider 
kinetic  optimization,  mass  moment  of  inertia,  smooth  velocity 
profiles  and  initial  and  final  velocities  at  zero. 

As  this  list  suggests,  the  possibilities  for  expanding 
this  research  are  enormous . 
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Block  Diagonal  Matrix 
4  Equations,  4  meshpoints 


Figure 


state  Variable  Trajectories:  Circular  Initial  Guess 


Costate  Variable  Trajectories:  Circular  Final  Solution 


Figure 


Costate  Variable  Trajectories:  10  Rolls,  5.5  Turns,  Final  Solution 


lambda  1&  2 


lambda  3 
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Figure  14 


Path  on  X-Y  Plane:  Robot  1 


X.  Y  .Theta 


Figure  1  8 


Time 
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X  ,Y  .Theta  Values 


Path  on  X-Y  Plane:  Robot  3 


Figure  19 


State  Variable  Trajectories:  Trivial  Solution 


Figure  20 


Time 
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Angle,  radians 


Path  with  Variation  of  Alpha,  Robotl 


Figure  23  X 


Angular  Trajectory  with  Variation  of  Alpha,  Robot! 
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Energy  Cost  with  Variation  of  Alpha,  Robotl 


Figure  25 
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Angle,  radians 


Path  with  Variation  of  C,  Robotl 


Angular  Trajectory  with  Variation  of  C,  Robotl 


Figure  27 
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Energy  Cost 


Energy  Cost  with  Variation  of  C,  Robotl 
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Angle,  radians 


Path  Comparison  of  Robot  Programs,  90  Degree  Turn 


Angle  Trajectory  Comparison  of  Robot  Programs,  90  Degree  Turn 
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Robot  Number 


Robot  Number 


Figure  31  (c) 


Figure  31  (d) 
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Angle,  radians 


Path  Comparison  of  Robot  Programs,  30  Degree  Turn 


Angle  Trajectory  Comparison  of  Robot  Programs,  30  Degree  Turn 
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Robot  Number 
Figure  35  (a) 


Robot  Number 
Figure  35  (b) 


Path  Comparison  of  Robot  Programs,  270  Degiee  Turn 


Angle  Trajectory  Comparison  of  Robot  Programs,  270  Degree  Turn 
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Velocity  Profiles:  Robot  5,  270  degree  turn 


s8!i!Ooi0A 


r- 
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Figure  38 


Figure  40  (a) 


Figure  40  (b) 


^q-5  Robot  3,  4  &  5 


Figure  40  (c) 
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Robot  Number 
Figure  41  (a) 


Robot  Number 
Figure  41  (b) 


Robot  1 


Robot  2  &  3 


Robot  1 


Robot  2  &  3 


)  0.5 

Time 

Figure  43  (a) 


D  0.5 

Time 

Figure  43  (b) 


Robot  4  Robot  5 


Time  Time 

Figure  43  (c)  Figure  43  (d) 
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APPENDIX  A 


Progrzun  Files  Specific  to  Disk 

GENDISK.FOR 
DIFEQ . FOR 
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PROGRAM  GENDISK 


*  This  is  the  most  general  and  most  interactive  form  of  the  disk  programs. 

*  It  works  for  problems  for  which  a  circular  initial  guess  is  most 
applicable . 

*  In  some  cases  the  alternate  straight  line  guess  may  be  more  applicable. 

*  There  is  an  apparent  singularity  for  the  exact  straight  line  case. 

*  Fortunately  this  case  is  readily  solved  by  analytically. 


Source  for  subroutines  Red,  Pinvs,  Solvde.  and  Bksub  and  model  for 
Difeq  and  Oiskroain: 

Numerical  Recipes,  William  H.  Press,  et  al 


IMPLICIT  REAL'S  (A-H,  0-Z) 

PARAMETER  (NE=8  ,  M  =  201 ,  NB  =  4  ,  NCI=NE,  NCJ=NE-NB<-1 ,  NCK=M+1 ,  NSI=NE, 
NSJ=2'NE+1,  NYJ=NE,  NYK=M) 


Variable  description; 
GENERAL  PROGRAM  VARIABLES: 


NE:  Number  of  independent  equations  describing  system 

M:  Number  of  Meshpoints,  divisions  of  independent  variable,  time 

NBr  Number  of  Boundary  Conditions  known  at  initial  condition 

C;  3-D  Array  for  storage  of  corrections  for  each  iteration 

Note:  largest  array  in  program 
NCI,  NCJ.  NCK: 

dimension  variables  of  C  array,  must  satisfy  equations 
found  in  parameter  statement 
S:  array  for  storage  of  blocks  of  solution  of  Difeq. 

NSI,  NSJ; 

dimension  variables  of  2-D  S  array,  must  satisfy  equations 
found  in  parameter  statement 

Y;  2-D  array  containing  initial  guess  for  each  point.  This  array 

is  updated  by  calculated  corrections.  When  the  corrections 
are  sufficiently  small,  convergence  is  acheived. 

X;  Array  for  independent  variable,  time.  Used  only  for  comparison 
of  dependent  variables  after  program  completes. 

SCALV;  Array  of  values  representing  the  typical  magnitude  of  the 

dependent  variables.  Used  for  controlling  correction  magnitude. 
INDEXV; lists  Column  ordering  for  variables  in  S  array,  not  used  in  this 
program 

ITMAX:  Maximum  number  of  iterations 

CONV:  Convergence  criteria  for  corrections  to  Y 

SLOWC;  Controls  fraction  of  corrections  applied  to  Y 

K:  Increment  of  independent  variable,  divisions  between  mesh  points 

PROBLEM  SPECIFIC  VARIABLES: 


RAD:  Radius  of  Disk 

ROLL:  Number  of  revolutions  the  disk  is  required  to  make 

TURNS;  Number  of  turns  the  disk  is  required  to  make 


DIMENSION  SCALV(NE) , INDEXV(NE) , Y(NE,M) , C (NCI , NCJ , NCK)  ,  S(NSI,NSJ) 
COMMON  X(M),  H,  RAD,  ROLL,  TURNS,  xo, xf , yo, yf , to , t f , po , pf 

OPEN(21, FILE='xplof ,  STATUS= ' UNKNOWN' ) 

OPEN(22,  FILE='  Iplof  ,  STATUS  =' UNKNOWN '  ) 

OPEN(30,FILE='xiplot' ,  STATUS =' UNKNOWN ' ) 

0PEN(31,  FILE='  itplof  ,  STATUS  =' UNKNOWN'  ) 

OPEN(32,FILE='testdat' ,  STATUS =' UNKNOWN ' ) 

OPEN(33, FILE=' liplot' ,  ST ATUS =' UNKNOWN ' ) 

H=1 . / (M-1) 

PI=  3  .  141592654 
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Print  *, 'Enter  Test  Number.' 

Read  • ,  ITESTNO 

Print  *,' Enter  the  maximum  number  of  iterations.' 
Read  * ,  ITMAX 

Print  *, 'Enter  the  convergence  criteria.' 

Read  *,  CONV 

Print  '.'Enter  SLOWC.' 

Read  *,  SLOWC 

Print  Enter  disk  radius.' 

Read  * ,  RAD 

Print  *, 'Enter  1  for  circular  initial  guess,' 

Print  *, 'Enter  2  for  straight  line  initial  guess,' 
Print  *, 'Enter  3  for  an  elliptical  initial  guess,' 
Read  *,  IGUES3 

IF ( IGUESS . EG . 3 ) THEN 

PRI’JT  'Enter  factor  for  width  of  elipse' 

Read  * ,  A 

PRINT  'Enter  factor  for  height  of  elipse' 
Read  * ,  B 
ENDIF 


C  Boundary  Conditions: 

Print  *, 'Enter  the  starting  X-Y  coordinates  of  the  disk. 
Read  *,  xo,yo 

Print  *, 'Enter  the  final  X-Y  coordinates  of  the  disk.' 
Read  ♦,  xf,yf 

Print  *, 'Enter  starting  theta  in  degrees.' 

Read  ',  to 
to  =  to'PI/180 

Print  ’.'Enter  starting  phi  in  degrees.' 

Read  * ,  po 
po  =  po*PI/180 

Print  *, 'Enter  the  number  of  required  rolls  of  the  disk. 

Read  ',  ROLL 

pf=  po  ♦  2*PI*RAD*R0LL 

Print  'Enter  the  number  of  required  turns  of  the  disk. 
Read  *,  TURNS 
tf=  to  *  2*PI*TURNS 

Write  (32,*)  'ITESTNO  =', ITESTNO 
Write  (32,*)  'ITMAX  =', ITMAX 
Write  (32,')  'CONV  =',CONV 
Write  (32,')  'SLOWC  =', SLOWC 
Write  (32.')  'RAD  = ' ,  PAD 
Write  (32,')  'ROLL  =',ROLL 
Write  (32,*)  'TURNS  ='. TURNS 

Write  (32,*)  'xo  = ' , xo 
Write  (32,*)  'yo  = ' , yo 
Write  (32,*)  'theta  0  =',to 
Write  (32,*)  'phi  0  =',po 
Write  (32,*)  'xf  =',xf 
Write  (32, *)  'yf  = ' ,yf 
Write  (32,*)  'theta  f  =',tf 
Write  (32,*)  'phi  f  =',pf 

C  NO  INDEX  CHANGES  NECESSARY 

INDEXV(1)=1 
INDEXV(2) =2 
INDEXV(3) =3 
INDEXV(4) =4 
INDEXV(5) =5 
INDEXV(6)  =:6 
INDEXV(7) =7 
INDEXV(8)  =8 


C  Initialize  independent  vector  X  (time) 

IX)  11  K=1,M 

X(K)=(K-1)*H 

11  CONTINUE 

C  Enter  initial  guess  values  for  all  meshpoints 

IF  ( IGUESS . EQ . 1 )  THEN 

C  CIRCULAR  INITIAL  GUESS  WITH  LAMBDA'S  NEAR  ICNOWN  CIRCLE  SOLUTION: 

DO  12  K=1,M 

y (3,K) =(K-1) •2*PI*TURNS/ (M-1) 

y(l,K)=ROLL*RAD*SIN(y(3, K> ) /TURNS 

y(2,K) =ROLL*RAD* (1-COS (y (3, K) ) ) /TURNS 

y (4,K) =(K-1) •2*PI*ROLL*RAD/ (M-1) 

y(5,K)=0 

y(6,K)=0 

y(7,K)=-2*PI 

y(8,K) =-2*PI*ROLL*RAD 

12  CONTINUE 

C  Scalv  set  to  approximate  size  of  typical  values  for  circle 

SCALV  ( 1 )  =ROLL  *  RAD  /  2 
SCALV ( 2 ) =ROLL  *  RAD 
SCALV(  3 )  =PI*TURNS 
SCALV(4) =ROLL*PI 
SCALV(5) =.05 
SCALV(6)=.05 
SCALV(7)=1 
SCALV(8)=10 

ELSEIF( IGUESS. EQ. 2)  THEN 

C  STRAIGHT  LINE  INITIAL  GUESS: 

DO  14  K=1,N 

y(l,K)=(xf-xo) *(K-l)/(M-l)*XO 

y(2,K)  =  (yf-yo)MK-l)/(M-l)*yo 

y(3,K)=ATAN( (xf-xo) / (yf-yo) ) 

y(4,K)=(K-l) •2*PI*R0LL/ (M-1) 

y(5,K)=7.77 

y(6,K)=7.77 

y(7,K)=0.001 

y(8,K)=-25. 13238 

14  CONTINUE 

C  Scalv  set  to  approximate  size  of  typical  values  for  LINE 
SCALV(1)=1 

SCALV  ( 2  )  =ROLL’*  RAD  •  P I 
SCALV (3) =1 

SCALV(4) =ROLL*RAD*PI 
SCALV(5)=1 
SCALV(6)=1 
SCALV(7) =1 
SCALV(8) =1 

ELSEIF  ( IGUESS . EQ . 3 )  THEN 

C  ELLIPTICAL  INITIAL  GUESS  WITH  LAMBDA'S  NEAR  ICNOWN  CIRCLE  SOLUTION- 

DO  15  K=1,M 

y (3,K) = (K-1) •2*PI*TURNS/ (M-1) 
y(l,K)=A*ROLL*RAD*SIN(y(3.K) ) /TURNS 
y(2,  K)=B*ROLL*RAD*{l-COS(y(3,  K)  )  )  /TURNS 
y(4,K) =(K-1) *2*PI*R0LL*RAD/ (M-1) 
y(5,K) =0 
y(6,K) =0 
y (7. K) =-2*PI 
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y(8,K) =-2*PI*R0LL*RAD 
15  CONTINUE 

C  Scalv  set  to  approximate  sue  of  typical  values  for  circle 

SCALV(l)  =ROLL*RAD.  2 
SCA1,VI2I  *ROLL*RAD 
SCALVI3) =PI •TURNS 
SCA1.V(4)  =ROH.*PI 
SCALVIS) » .05 
SCAI,V(6»  -  05 
SCALV(7)  .1 
SCALV ( i  I  •  1 0 

E3iDIF 

C  Write  initial  ijuess  file 

r»0  13  K  =  1  .  M 

WRITE (30.601  XIF'.Yil  f  f  2  F i . r  •  C •  V  4  n 
WRITE  (  3  3,80'  ,X  I F  l  .  Y  l  S  .  F  Y  6.Fi  Y  '  F.  Y  i  ‘ 

1)  CONTINUE 

80  FORNAT(2X. 5F10  5  i 


C  EXPLICIT  ENTRY  OF  BOUNDARY  CONDITIONS 

y ( 1 . 1 ) -xo 

yd.Ml  =x( 
y ( 2 . 1 ) »yo 
y  (2.m  =yf 
y ( 3 ,  1 ) =to 
y(3.Hl  =ti: 
y ( 4 , 1 1 »po 
y(4.N) -pf 


CALL  SOLVDEI  ITMAX.COfT/.  SLOWC.  SCALV.  :nDEX-.-  SF,.NB  **  Y  NY.3  r.YF 
•  C.NCI.NCJ.NCF.S  NSI.NSJl 

C  Write  final  Y  values  to  file 

DO  181  k  =  1.201 

WRITE(21.e3l  X(K!.Y(l,kiY<;  kl.Yt)  xt.Y14.ki 
WRITE(22 .831  X(Fi.Y(5.ki.Y(6k),Y(tk),Y(8xl 
181  CONTINUE 

83  FOR«AT(2x, 5F15 . 51 

PRINT  •.'PROGRAM  COMPLETED 

CLOSE! 21 1 
CLOSe(22l 
CLOSE ( 30) 

CLOSE) 31) 

CLOSE (32) 

CLOSE) 33) 

END 
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SUBPOUTINE  DIFEO  I  K  K1  .  k;  JFK  .  rsl  .  :SF.  INDEX'.',  NE.  S  N3:  Nr-'  V  NYJ 
NYK  ) 

IMPLICIT  REAL**  lA  H  O  I: 

PARAMETER  (MOOI  > 

DIMDiSIOS  riNYJ  NYRI  StNSI  NS-' •  INDEX'.'  NYC‘ 

rC***ON  X(M1  M  RAT  RDLI.  ''^L’RXIS  ;I 

PI  ^  DACOr'  I 
PI  •  } 

I  r  -  .  T  .  A  I  12**  I  I  i  »  s 


••  ;  r  I  - 1  wr  I 

tX'  »  N  r : 

S  !  I  C 
COMTINCr. 

DVT  I  HUE 


I-.it'Al  P-'^ridlary  Toryl  .  •  i---.* 
I r  P  EO  E I  THEN 


Er'rr  no'i 


:xi  ;  I  I  ; ,  4 

S  ■■  4  •  I  *  ■  I  -  1 
'-OKTIK'.'E 


In.::«l  in  r  ighl  Ejind  vo^-'ot  t  f 


S (5 . JSEi 
Sit. JSE! 
SO,  JSE  . 
S (8 . JSE' 


y  '  !  I  KO 

y  •  2  1  I  yo 
y  '  3 ,  1 r  to 
y ' 4  .  1  '  •  po 


Mwck 


End  Bound* t y  C ond it  ion* 

ELSE  IE  (X  OT  R2l  THEJ< 

Enter  non  lero  values 

DO  12  1=  1.4 

S(  1 . 8*  1 1  =1  0 
12  CONTINUE 

Einal  values  in  right  hand  vector  Cor  final  block 

S(l. JSE) =  yil.M) -xf 
S(2. JSE) =  y (2,M) -yf 
SO.  JSE)  =  yO.M)  -tf 
S(4. JSE)»  yit.M) -p( 


Interior  Points 

Derived  from  Finite  Difference  Equations  of  Motion 


ELSE 

Pre  •  ca  1  cu  lal  i  on  of  consnonly  used  variables: 

Y!-'fil,R!*YO,Kl) 

YS- Y  I  S,  El  'Y  (5.  R  -  1  ) 

Yt O I  6. R) -Y (6. R  1 ) 

Y1-YI7. R) ‘Yi ’, R  -  1  I 
Y»  -  Y  (  8  ,  R  1  •  Y  I  8  R  -  1  I 
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r  1 
z2 


Ent#r  non-i«ro  values 

s( : , 1 1  =  1 

S  1 1  .  2  )  =  0 

SC.ll  -  h*Bad**2*Y6*CostY3l  /  i4*r2)  h*Pad*  Y8  *  S  i  r>  I  Y  )  2  l  i  »  •  :  2  l 
s  .‘i*Bad**2*Y5*Sin(Y))  (4*r2l 

S  (  1  ,  4  )  -  0 

subs  -  h*Rad*  *2  /  I  4*r  2  '  •  h*Pad*  •  2 ‘Cos  I  Y  3 1  '  i  4  •  r . 

Sll  6)  -  h*I>ad”2*SiniY3  i  /  I4*t2  . 

S  ( 1  .  2  I  =  0 

SIS  »1  -  h*PadToslY3-2)  12T2; 

5(1.4)  -  i 
5(1.101  -  3 

S(lll)  =  s(1.3) 

S(1 , 12)  -  0 

5(113)  =  1(1 . b) 

5(1.14)  ^  ill.«) 

5(1.  lb)  0 
5(1.16)  :  1(1.8) 

5(2.1)  =  0 
5(2.2)  =  - : 

5(2.3)  •  h*Rad*Y8*Coi  (  Y3  2  )  /  (4T 2  )  •h'Rad*  •  2  * Yb Tos  ( Y  3  /  '  (  4  T 1  i  • 

k  h*l(ad**2"Y4*Sin(  Y3 1  /  (4  •r2) 

5(2.4)  >  0 

5(2.5)  «  h*IUd**2*Sin(Y3)  /  (4*r2) 

5(2.4)  «  h*Rad**2/ (4T2)  -  h'Rad*  •  2 'Cos  ( Y  3  >  <  ( 4  •  r  2  > 

5(2.2)  =  0 

5(2,8)  =  h*Rad*Sin(Y3/2)  /  (2T2) 

5(2.9)  »  0 
5(2.101  :  1 
5(2.11)  >  1(2.3) 

5(2.12)  <  0 
5(2.13)  <  1(1.6) 

5(2.14)  =  1(2.6) 

5(2.15)  =  0 
5(2.16)  >  1(2.6) 

5(3.1)  «  0 
5(3.2)  *  0 
5(3.3)  *  -1 
5(3.4)  =  0 
5(3.5)  =  0 
s;3,6)  =  0 
5(3,7)  =  h/ (2*rl) 

5(3,8)  =  0 
5(3,9)  =  0 
5(3,10)  =  0 
5(3,11)  =  1 
5(3,12)  =  0 
5(3,13)  =  0 
5(3,14)  =  0 
5(3,15)  =  s(3,7) 

5(3,16)  =  0 
5(4,1)  =  0 
5(4,2)  =  0 

5(4,3)  =  h*Rad*Y6*Cos  (Y3/2)  /  (4T2)  -  h*Rad*Y5*Sin ( Y3 /2  )  /  ( 4  * r2  ) 
5(4,4)  =  -1 

5(4,5)  =  h*Rad*Cos(Y3/2)/(2'r2) 

5(4,6)  =  h*Rad*5in(Y3/2) / (2*r2) 

5(4,7)  =  0 
5(4,8)  =  h/ (2*r2) 

5(4,9)  =  0 
5(4,10)  =  0 
5(4,11)  =  S(4,3) 

5(4,12)  =  1 
5(4,13)  =  s(l,8) 

5(4,14)  =  8(2, 8) 
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S(4,15)  =  0 
S(4,16)  =  s(4,8) 

S(5,l)  =  0 
S(5.2)  =  0 
S(5,3)  =  0 
S(5.4)  =  0 
S(5,5)  =  -1 
S(5,6)  =  0 
S(5,7)  =  0 
S(5,8)  =  0 
S(5,9)  =  0 
S(5,10)  =  0 
S(5, 11)  =  0 
S(5,12)  =  0 
S(5,13)  =  1 
S(5,14)  =  0 
S(5, 15)  =  0 
S(5.16)  =  0 
5(6,1)  =  0 
5(6.2)  =  0 
5(6,3)  =  0 
5(6.4)  =  0 
5(6,5)  =  0 
5(6,6)  =  -1 
5(6.7)  =  0 
5(6.8)  =  0 
5(6.9)  =  0 
5(6.10)  :  0 
5(6,11)  =  0 
5(6,12)  =  0 
5(6,13)  =  0 
5(6.14)  :  1 
5(6,15)  *  0 
5(6,16)  =  0 
5(7,1)  =  0 
5(7,2)  =  0 

5(7,3)  =  h*Rad*Y5*Y8*Cos(Y3/2)/(8T2). 
6  h*IUd**2*Y5**2*Cos(Y3)/(8T2)  - 

6  h*Rad**2*Y6**2*Coa  (Y3)  /  (8T2)  • 

6  h*Rad*Y6*Y8*Sin(Y3/2)  /  (8T2)  ♦ 

6  h*Rad**2*Y5*Y6*Sin(Y3)  /  (4T2) 


o  n 


S(8,14)  =  0 
S(8,15)  =  0 
S(8, 16)  =  1 

S(1,JSF)  =  h*Rad**2*Y5/ (4*r2)  ♦  h'Rad'YB'Cos ( Y3 / 2 ) / ( 2  * r2 )  ♦ 

S.  h*Rad**2*Y5*Cos(Y3)/(4*r2)  ♦ 

&  h*Rad**2*Y6*Sin(Y3) /(4*r2)  -  Y{1.-1  »  K)  ♦  Y(1,K) 

S(2,JSF)  =  h*Rad**2*Y6/ (4*r2)  -  h*Rad* *2*Y6*Cos ( Y3 ) / ( 4 •r2 )  ♦ 
&  h*Rad*Y8*Sin(Y3/2) / (2T2)  «  h*Rad* •2*Y5»Sin ( Y3 ) / ( 4 •r2 )  - 
&  Y(2,-l  ♦  K)  ♦  Y(2,K) 

S(3,JSF)  =  h*Y7/(2*rl)  -  Y(3.-l  »  K)  ♦  Y(3,K) 

S(4,JSF)  =  h*Y8/(2*r2)  *  h*Rad* YS'Cos ( Y3 /2 ) / ( 2 * r2 )  ♦ 

&  h*Rad*Y6*Sin(Y3/2)  /  (2T2)  -  Y(4,-l  ♦  K)  *  Y(4,K) 

S(5,JSF)  =  -Y(5,-l  ♦  K)  ♦  Y(5.K) 

S(6,JSF)  =  -Y(6,-l  ♦  K)  ♦  Y(6,K) 

S(7,JSF)  =  -  (h*Rad*Y6*Y8*Cos{Y3/2)  )  /  (4T2)  - 
^  h*Rad**2*Y5*Y6*Cos  (Y3)  /  (4T2)  ♦ 

&  h*Rad*Y5*Y8*Sin(Y3/2) / (4*r2)  ♦ 

(.  h*Rad**2*Y5*’2*Sin(Y3)  /  (8*r2)  - 

&  h*Rad**2*Y6**2*Sin(Y3) / (8T2)  -  Y(7,-l  .  K)  •  Y(7,K) 

S(8.JSF)  =  -Y(8,-l  ♦  K)  ♦  Y(8,K) 


ENDIF 


Dummy  use  of  variables  to  prevent  inocculous  warning  on  MS  Compiler 
(Variables  not  used) 

ISl  =  ISl 
ISF  =  ISF 

INDEXV(l)  =  INDEXV(l) 

NE  :  NE 

RETURN 

END 
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APPENDIX  B 


Program  Files  Specific  to  Robot  1 

ROBOTl . FOR 

DIFEQ.FOR  (for  Robot  1) 
XLATOR . FOR 
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PROGRAM  ROBOTl 


Source  for  subroutines  Red,  Pinvs,  Solvde,  and  Bksub  and  model  for 
Difeq  and  Diskmain: 

Numerical  Recipes,  William  K.  Press,  ec  al 


IMPLICIT  REAL'S  (A-H,  0-Z) 


PARAMETER  (NE= 6 ,  M=201 ,  NB  =  3  ,  NCI=NE,  NCJ=NE-NB->  1 ,  NCK=M»  1 ,  NSI=NE, 

&  NSJ=2*NE+1,  NYJ=NE,  NYK=M) 

DIMENSION  SCALV(NE)  ,  INDEXV(NE)  ,  Y(NE,M)  ,C  {NCI ,  NCJ ,  NCK)  ,  S  (NSI,NSJ)  , 
k  YDOT(NE-l,M) ,POS(NE-l,M) 

COMMON  X(M),  H,  DELXO,  DELYO,  DELTHETO,  THETAD,  EPS,  ALPHA,  CWT 


Variable  description: 
GENERAL  PROGRAM  VARIABLES: 


NE:  Number  of  independent  equations  describing  system 

M:  Number  of  Meshpoints,  divisions  of  independent  variable,  time 

NB :  Number  of  Boundary  Conditions  known  at  initial  condition 

C:  3-D  Array  for  storage  of  corrections  for  each  iteration 

Note:  largest  array  in  program 
NCI,  NCJ,  NCK: 

dimension  variables  of  C  array,  must  satisfy  equations 
found  in  parameter  statement 

S:  array  for  storage  of  blocks  of  solution  of  Difeq. 

NSI,  NSJ: 

dimension  variables  of  2-D  S  array,  must  satisfy  equations 
found  in  parameter  statement 

Y:  2-D  array  containing  initial  guess  for  each  point.  This  array 

is  updated  by  calculated  corrections,  when  the  corrections 
are  sufficiently  small,  convergence  is  acheived. 

X:  Array  for  independent  variable,  time.  Used  only  for  comparison 

of  dependent  variables  after  program  completes. 

SCALV:  Array  of  values  representing  the  typical  magnitude  of  the 

dependent  variables.  Used  for  controlling  correction  magnitude 
INDEXV: lists  column  ordering  for  variables  in  S  array,  not  used  in  this 
program 

ITHAX :  Maximum  nustber  of  iterations 

CONV:  Convergence  criteria  for  corrections  to  Y 

SLOWC :  Controls  fraction  of  corrections  applied  to  Y 

H:  Increment  of  independent  variable,  divisions  between  mesh  points 

PROBLEM  SPECIFIC  VARIABLES: 


ROBOT. 

ROBOT. 


XO. 

YO; 

THETAO : 
XF: 

YF: 

THETAD. 
DELXO : 
DELYO : 
DELTHETO : 
EPS. 

EPS2  : 


Initial  X  coordinate  of  robot 
Initial  Y  coordinate  of  robot 

Initial  angle  wrt  X  axis  of  robot 
Desired  final  X  coordinate  of  robot 
Desired  final  Y  coordinate  of  robot 

Desired  final  angle  coordinate  of  robot 
Initial  boundary  condition  for  state  variable  delta-x 

Initial  boundary  condition  for  state  variable  delta-Y 

Initial  boundary  condition  for  state  variable  delta-theta 

Smallest  value  for  which  f (delta-x) :Sin(delta-x) / (delta-xl 
DUMMY  VARIABLE  USED  FOR  CONTINUITY  WITH  OTHER  FORMS  OF 


ALPHA:  Rotational  gain  related  to  delta-theta-dot 

ALPHAMAX:  DUMMY  VARIABLE  USED  FOR  CONTINUITY  WITH  OTHER  FORMS  OF 


CWT:  Weighting  parameter  for  Terminal  Costs 

RL1,RL2,RL3:  Initial  guess  amplitude  for  lambda  costates. 

OHMl , 0HM2 , 0HM3 :  Initial  guess  frequencies  for  lambda  costates 

DC1,DC2,0C3:  Initial  guess  d.c.  offsets  for  lambda  costates. 
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o  o 


PHIl ,  PHI2  ,  PHI3  ;  Initial  guess  phase  lag  for  lajnbda  costates. 

SL1,SL2,SL3:  Initial  guess  SCALV,  scale  sizes,  for  Izunbda  costates. 

P0S(NE-1,M);  Position  Trajectory  for  x,y, theta,  xd  and  yd. 


0PEN(21,FILE=‘xplot.robl' ,  STATUS= ' UNKNOWN ' ) 
OPEN(22,FILE=-lplot.robl' ,  STATUS= ' UNKNOWN ' ) 
OPEN(30,FILE=’xiplot.robl' ,  STATUS =' UNKNOWN' ) 
0PEN(31,FILE=’liplot.robl' ,  STATUS =' UNKNOWN ' ) 
OPEN(32,FILE='itplot.robl‘ ,  STATUS=' UNKNOWN' ) 
OPEN(33,FILE='testdat.robl' .  STATUS =' UNKNOWN' ) 
OPEN(34,FILE='dotplot.robl' ,  STATUS = 'UNKNOWN ' ) 
OPEN(35,FILE='posplot.robl' ,  STATUS=' UNKNOWN' ! 

OPEN(37, FILE='ulplot .robl ' ,  STATUS=' UNKNOWN' ) 

PI=  3.141592654 

PRINT  *, 'ENTER  ITMAX,  CONV,  SLOWC ' 

READ  ♦,  ITMAX,  CONV,  SLOWC 
PRINT  •, 'ENTER  ALPHA' 

READ  *.  ALPHA 

print  *, 'DUMMY  READ  FOR  DATA  FILE  COMPATABILITY,  Enter  #' 
READ  •,  DUMMY 

PRINT  *,  'ENTER  C,  WEIGHTING  PARAMTER  FOR  TERMINAL  COST' 
READ  *,  CWT 

PRINT  *, 'ENTER  EPSILON' 

READ  * ,  EPS 

print  ♦, 'DUMMY  READ  FOR  DATA  FILE  COMPATABILITY,  Enter  »' 
READ  ♦,  DUMMY 

PRINT  ',  'ENTER  INITIAL  X, Y,  AND  THETA (degrees ) ' 

READ  *,  XO.YO.THETAO 
THETAO  =THETA0 • P I / 1 8  0 

PRINT  *,  'ENTER  FINAL  X,Y,  AND  THETA (degrees ) ' 

READ  *,  XF,YF,THETAD 
THETAD=THETAD*PI/180 

PRINT  ♦ , ' ENTER  INITIAL  GUESS  FOR  3  LAMBDA  AMPLITUDES ' 

READ  *,  RL1,RL2,RL3 

PRINT  *, 'ENTER  INITIAL  GUESS  FOR  3  LAMBDA  FREQUENCIES' 

READ  ♦,  OHMl,OHM2,OHM3 

PRINT  *, 'ENTER  INITIAL  GUESS  FOR  3  LAMBDA  DC  OFFSETS' 

READ  *,  DC1,DC2,DC3 

PRINT  *, 'ENTER  INITIAL  GUESS  FOR  3  LAMBDA  PHASES' 

READ  *,  PHIl,  PHI2,  PHI3 
PHIl  =  PHIl*PI,a80 
PHI2=PHI2*PI/180 
PHI3=PHI3*PI/180 

PRINT  *, 'ENTER  INITIAL  GUESS  FOR  SIZE  OF  3  LAMBDA  VALUES' 
READ  ♦,  SL1,SL2,SL3 

DELX0=(XF-X0) 

DELY0=(YF-Y0) 

DELTHET0= (THETAD-THETAO ) 


H=1 ./ (M-1) 

NO  INDEX  CHANGES  NECESSARY 
Index  Scale  used  by  SOLVOE.FOR 

INDEXV(l) =1 
INDEXV(2) =2 
INDEXV(3) =3 
INDEXV(4) =4 
INDEXV(5) =5 
INDEXV(6) =6 

c  INITIAL  GUESS  FOR  ALL  POINTS,  1  -  M 

C  Initialize  independent  vector  X  (time) 
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noon 


DO  11  K=1,M 

X(K) = (K-1) ‘H 

11  CONTINUE 

Enter  initial  values  for  all  meshpoints 

NOTE:  BOUNDARY  CONDITONS  FOR  Y(l)-Y(3)  ARE 

ENTERED  AT  POINTS  1  AND  M  DURING  THE  INITIAL  GUESS  I  I  ! 
THESE  NUMBERS  MUST  COINCIDE  WITH  ANY  DESIRED  B.C. ! ! I 

C  INITIAL  GUESS: 

DO  12  K=1,M 

y(l, K) =DELXO- (DELXO* (K-1) / (M-1) ) 
y (2,K) =DELY0- (OELYO* (K-1) / (M-1) ) 
y ( 3 , K ) =DELTHET0 - ( DELTHETO •(K-1)/(M-1)) 
y(4.K) =RL1*SIN(2*PI*0HM1* (K-1) / (M-1) ♦PHIl) ♦DCl 
y (5,K) =RL2*SIN(2*PI*OHM2* (K-1) / (M-l ) ♦PHI2 ) *002 
y(6,  K)=RL3*SIN(2*PI*OHM3*  (K-1)/(M-1)  ♦PHI3)  <-DC3 

12  CONTINUE 

C  Write  initial  guess  to  file 
DO  13  K=1,M 

WRITE (30, 80)  X(K) , Y(1,K) , Y{2,K) ,Y(3,K) 

WRITE (31, 80)  X(K),Y(4,K),Y(5,K),Y(6,K) 

13  CONTINUE 

C  Scalv  set  to  approximate  size  of  typical  values  of  known  solution 

SCALV(l) =ABS(DELX0/2)+.01 
SCALV ( 2 ) = ABS ( DEL YO / 2 ) ♦ . 0 1 
SCALV ( 3 ) =ABS ( 2  *  DELTHETO /M ) *.01 
SCALV(4)=ABS(SL1)*.01 
SCALV ( 5 ) = ABS ( SL2 ) ♦ . 0 1 
SCALV(6)=ABS(SL3)+.01 

C  WRITE  TEST  DATA  TO  FILE 

WRITE(33,*)  ' ITMAX  = ' , ITMAX 

WRITE (3 3,*)  'CONV  =',CONV 
WRITE (3 3,*)  ‘ SLOWC  = ' , SLOWC 
WRITE(33,*)  'ALPHA  =', ALPHA 
WRITE(33,*)  'CWT  =',CWT 
WRITE (33,*)  'EPSILON  ='.EPS 

WRrTE(33,')  'INITIAL  X,Y  &  THETA  =  ',  XO , YO ,  (THETAO *  180/PI ) 
WRITE(33,*)  'FINAL  X,Y  (.  THETA  =  ',  XF ,  YF,  (THETAD*  180/ PI ) 

WRITE (3 3,*)  'INITIAL  LAMBDA  VALUES',  RL1,RL2,RL3 
WRITE(33,*)  'SIZE  OF  LAMBDA  VALUES',  SL1,SL2,SL3 
WRITE(33,*)  'DELXO  =',  DELXO 
WRITE(33,*)  'DELYO  =',  DELYO 
WRITE(33,*)  'DELTHETO  =',  DELTHETO 

CALL  SOLVDE  ( ITMAX ,  CONV ,  SLOWC ,  SCALV ,  INDEXV ,  NE ,  NB ,  M ,  Y ,  NY J ,  NYK , 

*  C,NCI,NCJ.NCK,S,NSI,NSJ) 

C  Write  final  Y  values  to  file: 

DO  181  k  =  1,M 

WRITE (2 1,80)  X(K)  ,Y(l,k)  ,Y(2,k)  ,Y(3,k) 

WRITE (22, 80)  X (K) , Y ( 4 , k) , Y (5 , k) , Y ( 6 . k) 

181  CONTINUE 

80  FORMAT(2X,4F15.4) 

CALL  XLATOR  ( Y ,  YDOT ,  H ,  NY  J ,  NYK ,  X ,  THETAD ,  EPS ,  ALPHA , 

S.  XO,YO,  THETAO,  XF,YF,POS) 

PRINT  *, 'PROGRAM  COMPLETED' 

CLOSE (21) 

CLOSE (22) 
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CLOSE (30) 
CLOSE (31) 
CLOSE (32) 
CLOSE (33) 
CLOSE (34) 
CLOSE (35) 
CLOSE (37) 

END 
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SUBROUTINE  DIFEQ(K, K1,K2, JSF, ISl. ISF, INDEXV, NE, S , NSI , NSJ , Y , NYJ , 
*  NYK) 

IMPLICIT  REAL'S  (A-H,  0-Z) 


MODIFIED  7/18/94  TO  INCLUDE  TERMINAL  COST  AT  FINA  BOUNDARY  CONDITION 


PARAMETER (M=201 I 

DIMENSION  Y (NYJ. NYK) ,  S(NSI,NSJ),  INDEXV(NYJ) 

COMMON  X(M),  H,  DELXO ,  DELYO,  DELTHETO,  THETAD,  EPS,  ALPHA,  CWT 


C  Initialize  matrix  S  as  0 

DO  10  1=1, NSI 

DO  9  J=1,NSJ 

S(I,J)  =  0.0 

9  CONTINUE 

10  CONTINUE 


C  Initial  Boundary  Conditions 

IF(K.EQ.Kl)  THEN 

C  Enter  non-zero  values: 

DO  11  1=  1,3 

S(3*I,6t-I)=1.0 
11  CONTINUE 


C  Initial  values  in  right  hand  vector  for  initial  block 

S(4, JSF) =  y(l,  1) -DELXO 
S(5, JSF) =  y(2, 1) -DELYO 
S(6,JSF)=  y ( 3, 1) -DELTHETO 


C  End  Boundary  Conditions 

ELSE  IF  (K.GT.K2)  THEN 

C  Enter  non-zero  values: 

S(l,7)=  CWT 
S(l,8)=  0.0 
S(l,9)=  0.0 
S(l,10)=  -1.0 
S(l,ll)=  0.0 
S(l,12)=  0.0 

S(2,7)=  0.0 
S(2,8)=  CWT 
S(2,9)=  0.0 
S(2,10)=  0.0 
S(2,ll)=  -1.0 
S(2,12)=  0.0 

S(3,7)=  0.0 
S(3,8)=  0.0 
S(3.9)=  CWT 
S(3,10)=  0.0 
S(3,ll)=  0.0 
S(3,12)=  -1.0 

C  Final  values  in  right  hand  vector  for  final  bloc)t 
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o  o 


S(1,JSF)=  Y(1,M)*CWT  -  Y(4,M) 
S{2,JSF)=  Y(2,M)*CWT  -  Y(S,M) 
S{3,JSF)=  Y(3,M)*CWT  -  Y(6,M1 


Interior  Points 

Derived  from  Finite  Difference  Equations  of  Motion 


ELSE 

C  Pre-calculation  of  commonly  used  variables: 

Yl=(Y(i,K)+Y(l,K-l) )/2.0 
Y2=(Y(2,K)+Y(2,K-1) ) /2 .0 
Y3=(Y(3,K)+Y(3,K-l))/2.0 
Y4=(Y(4,K)+Y(4,K-1) )/2.0 
Y5=(Y(5,K)+Y(5,K-1) )/2.0 
Y6=(Y(6,K)+Y(6,K-1) )/2.0 

CTD=C0S (THETAD) 

STD=SIN(THETAD) 

CTDY3=COS (THErAD-Y3 ) 

STDY3=SIN 'THETAD-Y3 ) 

p_  y2*CTD-Y1*STD 

IF(ABS(Y3) .GT. EPS) THEN 
FY3=SIN(Y3) / (Y3) 

FPY3=(COS(Y3)/Y3)-(SIN(Y3)/(Y3**2)) 
SIG4=FPY3/2 .0 

SIG12= (-SIN(Y3) /Y3-2 .0*COS{Y3)/(Y3) ••2  ♦ 

&  2 .0*SIN(Y3) / (Y3) ••3) /2.0 

ELSE 

FY3=1.0 

FPY3=0.0 

SIG4=0.0 

SIG12=-l/6.0 

ENDIF 

U1=Y4*CTDY3  ♦  Y5*STDY3  +  Y6*P*FY3 
UD=:-Y4*CTD-Y5*STD 

SIG1=  -Y6*FY3*STD/2 .0 
SIG2=  Y6*FY3*CTD/2.0 

SIG3=  Y4/2.0*STDY3  -  Y5/2.0*CTDY3  ♦  Y6*P*SIG4 

SIG5=  -CTD/2.0 
SIG6=  CTDY3/2.0 
SIG7=  -STD/2.0 
SIG8=  STDY3/2.0 
SIG9=  P*FY3/2.0 
SIGIO  =  -STD/2.0 
SIG11=  CTD/2.0 

c  Enter  non-zero  values; 

S(l,l)=  -1  ♦  H*CTDY3*SIG1 
S(l,2)=  i*CTDY3*SIG2 
S(l,3)=  H* (CTDY3*SIG3  *  STDY3*Ul/2 .0) 

S(l,4)=  -H*(CTD*SIG5  -  CTDY3*SIG6) 

S(l,5)=  -H*(CTD*SrG7  -  CTDY3*SIG8) 

S(l,6)=  H*CTDY3*SIG9 
S(l,7)=  S(l,l)  ♦  2.0 
S(l,8)=  S(l,2) 

S(l,9)=  S(l,3) 

S(l,10)=  S(l,4) 

S(l,ll)=  S(l,5) 

S(l,12)=  S(l,6) 
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S(2, 1) =  H*STDY3*SIG1 

S(2,2)=  -1  +  H*STDy3*SIG2 

S(2,3)=  H*(STDY3*SIG3  -  CTDY3*Ul/2 . 0) 

S(2,4)=  H* (STDY3*S1G6-STD*SIG5) 

S[2,5)=  H* (STDY3*SIG8-STD*SIG7) 

S(2,6)=  H*STDY3*SIG9 
S(2,7)=  S(2,l) 

S(2,8)=  S(2,2)  ♦  2.0 
S(2,9)=  S(2,3) 

S(2,10)=  S(2,4) 

S(2,ll)=  S(2,5) 

S<2,12)=  S(2,6) 

S(3,l)=  H*FY3* (P*SIG1*SIG10*U1) 

S(3,2)=  H*FY3* (P*SIG2»SIG11*U1) 

S(3,3)=  -1  +  H*(ALPHA/2.0  P-(01*SIG4  ♦  SIG3*FY3)) 

S(3,4)=  H*P*FY3*SIG6 

S(3,5)=  H*P*FY3*SrG8 

S(3,6)=  H*P*FY3*SIG9 

S(3,7)=  S(3,l) 

S(3,8)=  S(3,2) 

S(3,9)=  S(3,3)  ♦  2.0 
S(3,10)=  S(3,4) 

S(3,ll)=  S(3,5) 

S(3,12)=  S(3,6) 

St4,l)=  H*Y6*STD*FY3*SIG1 
S(4,2)=  H*Y6*STD*FY3*SIG2 
S(4,3)=  H*Y6*STD* (U1*SIG4  ♦  SIG3*FY3) 

S(4,4)=  -1  +  H*Y6*STD*FY3*SIG6 
S(4,5)=  H*Y6*STD*FY3*SIG8 
S(4,6)=  H*STD*FY3* (Y6*S1G9  ♦  Ul/2.0) 

S(4,7)=  S(4,l) 

S(4,8)=  S(4,2) 

S(4,9)=  S(4,3) 

S(4,10)=  S(4,4)  ♦  2.0 
S(4,ll)=  S(4,5) 

S(4,12)=  S(4,6) 

S(5,l)=  -H*Y6*CTD*FY3*SIG1 
S(S,2)=  -H*Y6*CTD*FY3*SIG2 
S(5,3)=  -H*Y6*CTD*(U1*SIG4  *  SIG3*FY3) 

S{5,4)=  -H*Y6*CTD*SIG6*FY3 
S(5,5)=  -1  -  H*Y6*CTD*SIG8*FY3 
S(5,6)=  -H*CTD*FY3* (Y6*SIG9  +  Ul/2.0) 

S(5,7)=  S(5,l) 

S(5,8)=  S(5.2) 

S(5,9)=  S(5,3) 

S(S,10)=  S(5,4) 

S(5,ll)=  S(5,5)  +  2.0 
S(5,12)=  S(5,6) 

S(6,l)=  -H* (Y4*STDY3*SIG1  -  Y5*CTDY3 *SIG1  * 

Y6*FPY3* (P*SIG1*SIG10*U1) ) 

S(6,2)=  -H* (Y4*STDY3*SIG2  -  Y5*CTDY3 *S1G2  * 

Y6*FPY3* (P*SIG2*SIG11*U1) ) 

S(6,3)=  -H* (Y4* (-Ul*CTDY3/2 .0  ♦  S1G3*STDY3) 

-  Y5*(Ul*STDY3/2.0  ♦  CTDY3*SIG3) 

♦  Y6*P* (U1*SIG12  ♦  SIG3*FPY3)) 

S(6,4)=  -H* (STDY3* (Y4*SIG6  +  Ul/2.0)  -  Y5*CTDY3*SIG6  ♦ 
Y6*P*FPY3*SIG6) 

S(6,5)=  -H* (Y4*STDY3*SIG8  -  CTDY3* (Y5*SIG8  »  Ul/2.0)  - 
Y6*P*FPY3*SIG8) 

S(6,6)=  -1  -H* (Y4*STDY3*SIG9  -  y5*CTDY3 •SIG9 

♦Y6*P*FPY3*SIG9  ♦  (ALPHA* P*U1 *FPY3 ) /2 . 0 ) 

S(6,7)=  S(6,l) 

S(6,8)=  S(6,2) 

S(6,9)=  S(6.3) 

S(6,10)=  S(6,4) 
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S(6. 11) =  S(6.5) 

S(6, 12) r  SI6, 6)  ♦  2  0 


S(1,JSF)= 
S(2. JSF) = 
S(3.  JSF) 
S(4,  JSF)  = 
S(5. JSF) = 
S(6, JSF) = 


Y(1.K)-Y(1.K-1)-H* (CTD*UD'CTDY3*U1 ) 
Y(2.K)-Y(2.K-1) -H* (STD*UD-STDY3*U1 ) 
Y(3,K)-Y(3.K-1)*H* ( ALPHA* Y3*P*U1 •FY3 ) 

Y(4.K) -Y(4.K-1)*H*Y6*U1*STD*FY3 
Y(5.K)-Y(5.K-1) -H*Y6*U1*CTD*FY3 
y(6,K)-Y(6,K-l)-H* (Y4*U1*STDY3-Y5*U1*CTDY3* 
Y6* (ALPHA* P*U1*FPY3) ) 


ENDIF 


Dununy  use  of  variables  to  prevent  inocculous  warning  on  MS  Compiler 
(Variables  not  used) 

ISl  =  ISl 
ISF  =  ISF 

INDElCVd)  =  INDEXV(l) 

NE  =  NE 

RETURN 

END 
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SUBROUT  I NE  XLATOR  (  Y  .  YDOT ,  H ,  NY  J  .  N-YK  .  X .  THETAD ,  E  PS  .  AL  PHA 
k  XO , YO. THETAO. XF. YE, POSi 


This  subroutine  converts  the  state  variables  delta  x.y. theta 
into  Robot  variables:  x.y. theca  and  Virtual  Robot  varialbles  xd.  yd 
A  simple  trapezoidal  integration  is  used  to  determine  position  and 
energy  costs  by  integration  of  the  term  lul • *2 ) /2 -a Ipha 


IMPLICIT  REAL'S  (A-H.  0-2) 

PARAMETER (Mx 201 ) 

DIMENSION  Y(NYJ,NYK) ,  YDOT(5,M),  X(M),  POS(5,M), 
L  COST(H)  .NRCIM)  .UITRAJ(M)  .UDTRAJIM) 

INITIAL  POSITIONS  4  DJERGY  CONDITIONS 


POSd,  1)  «X0 
POS(2, 1) =Y0 
POS (3,1) =THETA0 
POS(4, 1) =XF 
pos(5, i)=yr 

NRG(l) =0 . 

DO  10  K=1,M 

IF(ABS(Y(3,K) ) .GT.EPSITHEN 
FY3=  SIN(Y(3,K))/Y(3.K) 

ELSE 

FY3=  1.0 
ENDIF 

CALCULATION  OF  VELOCITIES  Ul.UD 


Pdel=  Y(2,K) 'COS (THETAD) -Y(1,K) 'SIN (THETAD) 
Ul=  Y(4,K) *COS(THETAD-Y(3,K) )  ♦ 

4  Y(5,K)'SIN(THETAD-Y(3,K))  ♦ 

4  Y(6,K) 'Pdel'FYl 

UITRAJ(K) =U1 

UD=  -Y(4,K) 'COS (THETAD) -Y( 5, K) 'SIN (THETAD) 
UDTRAJ(K)  =UD 

CALCULATION  OF  ENERGY  COSTS 


*  ENERGY  COST  FUCTION  FOR  ROBOT4  4  2 

*  NRG(K)  =  (UITRAJ(K)  "2*UDTRAJ(K)  "2) /2 

*  ENERGY  COST  FUNCTION  IN  FORM  FOR  ROBOTS  (used  for  comparison) 
NRG(K)  =  (UlTRAJ(K)"2+UDTRAJ(K)"2)/2  ♦  ALPHA 

YDOT(l,K)=  COS(THETAD-Y(3,K))'01 
YDOT(2,K)=  SIN(THETAD-Y(3,K) ) *01 
YDOT(3,K>=  Pdel*Ul*FY3  ♦  ALPHA*Y(3,K) 

YDOT(4,K)=  COS (THETAD) *UD 
YDOT(5,K)=  SIN (THETAD) *UD 
C 

C  TRAPEZOIDAL  INTEGRATION : 


IF(K.GT.1)THEN 

POS (l,K)=POS(l,K-l)+H' (YDOT (1,K) 'YDOT ( 1, K-1 ) ) /2 
P0S(2,K)=P0S(2,K-1)+H' ( YDOT (2 , K) 'YDOT ( 2 , K-1 ) ) /2 
POS(3,K) =POS(3,K-l)*H' (YDOT(3,K) *YDOT(3,K-l) ) 12 
POS(4,K)=POS(4,K-l)*H'(YDOT(4,K)*YDOT(4,K-l) ) /2 
POS(5,K)=POS(5,K-l)+H'(YDOT(5,K)*YDOT(5,K-l) ) /2 

COST(K) =COST(K-l)+H* (NRG (K) +NRG ( K- 1 ) ) /2 
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ENOIF 


WRITE {34. 80)  X(K) . rDOT( 1 . Kl , TDOT(2. K) . YDOT( 3 . K) , rDOT(4 . K) . 
fc  YDOT(5,K) 

WRITE (3b. 80)  X(K1 . POS ( 1 . K) . POS)2 . K)  POS(3. K) . POS(4. K) . 
i.  POS(5.K) 

WRITE ( 31 , 81 )  X(K) .DITRAJ (K) . UDTRAJ)K) .COST)K) 

10  CONTINUE 

80  FORMAT (2X.6Flb. 4) 

81  FORMAT(2X. 4F15 . 4) 

RETURN 

END 


1 
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APPENDIX  C 

Program  Files  Specific  to  Robot  2 

R0B0T2 . FOR 

DIFEQ.FOR  (for  Robot  2) 
XLAT0R2 . FOR 
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PROGRAM  ROBOT2 


n  o 


G4T:  Weighting  parameter  for  Terminal  Costs 

RL1,RL2,RL3:  Initial  guess  amplitude  for  lambda  costates. 

OHHl , 0HM2 , 0HM3  :  Initial  guess  frequencies  for  lambda  costates. 

DC1,DC2,DC3:  Initial  guess  d.c.  offsets  for  lambda  costates. 

PHIl , PHI2 , PHI3 :  Initial  guess  phase  lag  for  lambda  costates. 

SL1.SL2,SL3;  Initial  guess  SCALV,  scale  sizes,  for  lambda  costates. 
P0S(NE-1,M):  Position  Trajectory  for  x,y, theta,  xd  and  yd. 


OPEN(21,FILE='xplot.rob2' ,  ST ATUS= 'UNKNOWN ' ) 
OPEN(22,FILE='lplot.rob2' .  STATOS= 'UNKNOWN ' ) 
0PEN(30,FILE='xiplot.rob2' ,  STATUS* 'UNKNOWN' ) 

OPEN (31, FILE* ‘ 1 iplot . rob2 ' ,  STATUS* ' UNKNOWN ' ) 

OPEN (32, FILE* ‘ itplot. rob2 ' ,  STATUS* 'UNKNOWN' ) 

OPEN (33, FILE*' testdat. rob2 ' ,  STATUS* 'UNKNOWN' ) 

OPEN  (34,  FILE* '  dotplot.  rob2  ' ,  STA'TUS*' UNKNOWN'  ) 
0PEN(35,FILE*'pO8plot.rob2' ,  STATUS* 'UNKNOWN' ) 
OPEN(37,FILE*'ulplot.rob2',  STATUS* 'UNKNOWN ' ) 

PI*  3.1415926S4 

PRINT  *, 'ENTER  ITMAX,  CONV,  SLOWC' 

READ  •,  ITMAX,  CONV,  SLOWC 
PRINT  ♦, 'ENTER  ALPHA' 

READ  *,  ALPHA 

print  *, 'DUMMY  READ  FOR  COMPATABILITY,  enter  •' 

READ  >,  DUMMY 

PRINT  *,  'ENTER  C,  WEIGHTING  PARAMTER  FOR  TERMINAL  COST' 
READ  *,  CWT 

PRINT  *, 'ENTER  EPSILON' 

READ  *,  EPS 

print  •,  'DUMMY  READ  FOR  COMPATABILITY,  enter  »' 

READ  *,  DUMMY 

PRINT  *,  'ENTER  INITIAL  X,Y,  AND  THETA(degrees ) ' 

READ  •,  XO,YO,THETAO 
THETA0=THETA0»PI/180 

PRINT  *,  'ENTER  PINAL  X,Y,  AND  THETA (degrees ) ' 

READ  •,  XF,Yr,THETAD 
THETAD=THETAD*  PI / 180 

PRINT  ♦,  'ENTER  INITIAL  GUESS  FOR  3  LAMBDA  AMPLITUDES' 
READ  *.  RL1,RL2,RL3 

PRINT  *,  'OTTER  INITIAL  GUESS  FOR  3  LAMBDA  FREQUENCIES' 
READ  *,  OKMl,OKM2,OKM3 

PRINT  •,  'OTTER  INITIAL  GUESS  FOR  3  LAMBDA  DC  OFFSETS' 

READ  *,  DC1,DC2,DC3 

PRINT  ♦,  'ENTER  INITIAL  GUESS  FOR  3  LAMBDA  PHASES' 

READ  *,  PHIl,  PHI2,  PHI3 
PHI1»PHI1*PI/180 
PHI2*PHI2*PI/180 
PHI3=PHI3*PI/180 

PRINT  *,  'ENTER  INITIAL  GUESS  FOR  SIZE  OF  3  LAMBDA  VALUES' 
READ  *,  SL1,SL2,SL3 

DELX0=(XF-X0» 

DELY0=(YF-Y0) 

DELTHETO* (THETAD-THETAO) 

H=1./(M-1) 

NO  INDEX  CHANGES  NECESSARY 
Index  Scale  used  by  SOLVDE.FOR 

INDEXV(1)*1 
INDEXV(2) *2 
INDEXV(3I *3 
INDEXV(4) =4 
INDEXV(5)*5 
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INDEXV(6) =6 


c  INITIAL  GUESS  FOR  ALL  POINTS,  1  -  M 

C  Initialize  independent  vector  X  (time) 

DO  11  K=1,M 

X(K) = (K-1) 'H 

1 1  CONTINUE 

Enter  initial  values  for  all  meshpoints 

NOTE:  BOUNDARY  CONDITONS  FOR  Y(l)-Y(3)  ARE 

ENTERED  AT  POINTS  1  AND  M  DURING  THE  INITIAL  GUESS ! 

THESE  NUMBERS  MUST  COINCIDE  WITH  ANY  DESIRED  B.C.!!! 

C  INITIAL  GUESS: 

DO  12  K^l.M 

y (1,K) =DELX0- (DELXO* (K-1) / (M-1) ) 
y(2. K)=DELYO- (DELYO* (K-1) / (M-1) ) 
y(3.K) =DELTHET0- (DELTHETO* (K-1) / (M-1) ) 
y(4,K)=RLl*SIN(2*PI*OHMl-(K-l)/(M-l)*PHIl)*DCl 
y)5,K)=RL2*SIN(2*Pl*OHM2* (K-1)/(M-1)»PHI2)*DC2 
y(6,K) =RL3*SIN(2*PI*OHM3* (K-1) /(M-1)»PHI3) *003 

12  CONTINUE 

C  Write  initial  guess  to  file 
DO  13  K=1,M 

WRITE (30, 80)  X(K) ,Y(1,K) ,Y(2,K) ,Y(3,K) 

WRITE (31, 80)  X(K) ,Y(4,K) , Y(5,K) ,Y(6,K) 

13  CONTINUE 

C  Scalv  set  to  approximate  size  of  typical  values  of  )cnovm  solution 

SCALV (1 ) sAfiS ( DELXO / 2 ) « . 0 1 
SCALV ( 2 ) =ABS ( DELYO / 2 ) ♦ . 0 1 
SCALV ( 3 ) =ABS ( 2 •DELTHETO /M ) * . 0 1 
SCALV(4)3ABS(SL1)*.01 
SCALV ( S ) cABS ( SL2 )*. 0 1 
SCALV ( 6 ) =ABS ( SL3 ) « . 0 1 

C  WRITE  TEST  DATA  TO  FILE 

WRITE(33,*)  'ITMAX  = ' , ITMAX 
WRITE (3 3,*)  'CONV  = ' , CONV 
WRITE(33,*)  'SLOWC  =',SLOWC 
WRITE133,*)  ' ALPHA  =•, ALPHA 
WRITE(33,*)  'CWT=!',CWT 
WRITE(33,*)  'EPSILON  =',  EPS 

WRITE(33,*)  'INITIAL  X,Y  4  THETA  =  ',  XO, YO, (THETA0*180/PI) 
WRITE(33,*)  'FINAL  X,Y  4  THETA  =  ',  XF, YF, (THETAD* 180/PI ) 
WRITE(33,*)  ' INITIAL  LAMBDA  VALUES ' .  RL1,RL2,RL3 
WRITE(33,*)  'SIZE  OF  LAMBDA  VALUES' ,  SL1,SL2,SL3 
WRITE(33,*)  'DELXO  =',  DELXO 
WRITE(33,')  'DELYO  =',  DELYO 
WR ITE ( 3  3 , * )  ' DELTHETO  = ' ,  DELTHETO 

CALL  SOLVDE  ( ITMAX ,  CONV,  SLOWC ,  SCALV,  INDEXV ,  NE ,  NB ,  M ,  Y ,  NY J ,  NYK , 

•  C,NCI,NCJ,NCK,S,NSI,NSJ) 

C  Write  final  Y  values  to  file: 

DO  181  )(  =  1,M 

WRITE (21, 80)  X(K)  ,Y(l,)t)  ,Y(2,)t)  ,Y(3,)t) 

WRITE (22, 80)  X (K)  ,  Y  ( 4 ,  )t)  ,  Y (5 ,  )c)  ,  Y ( 6 ,  )t) 

181  CONTINUE 

80  FORMAT (2X,4F15. 4) 

CALL  XLATOR2  ( Y ,  YDOT ,  H ,  NY  J ,  NYK  ,X,THETAD, EPS, ALPHA, 
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XO. YO,THETAO,XF, YF, POS) 


PRINT  *, 'PROGRAM  COMPLETED’ 

CLOSE (21) 

CLOSE (22) 

CLOSE (30) 

CLOSE (31) 

CLOSE (32) 

CLOSE (33) 

CLOSE (34) 

CLOSE (35) 

CLOSE (37) 


SUBROUTINE  DIFEQ ( K, K1 , K2 , JSF. ISl, ISF, INDEXV, NE, S , NSI , NSJ, Y , NYJ  , 
NYK) 

IMPLICIT  REAL'S  (A-H,  0-Z) 


*  MODIFIED  7/18/94  TO  INCLUDE  TERMINAL  COST  AT  FINA  BOUNDARY  CONDITION 

•  MODIFIED  7/21/94  FOR  NON-MOVING  VIRTUAL  (TARGET)  ROBOT 


PARAMETER (M=201) 

DIMENSION  Y( NYJ. NYK)  ,  S(NSI,NSJ),  INDEXVOTYJ) 

COMMON  X(M),  H,  DELXO,  DELYO,  DELTHETO,  THETAD,  EPS,  ALPHA,  CWT 


C  Initialize  matrix  S  as  0 

DO  10  1=1, NSI 

DO  9  J=1,NSJ 

S(I.J)  =  0.0 

9  CONTINUE 

10  CONTINUE 


C  Initial  Boundary  Conditions 

IF(K.EO.Kl)  THEN 

C  Enter  non-zero  values: 

DO  11  1=  1,3 

S(3+I,6*I)=1.0 
11  CONTINUE 

C  Initial  values  in  right  hand  vector  for  initial  bloc)t 

S(4,JSF)=  y(l,l)-DELX0 
3(5,JSF)=  y(2,l)-DELY0 
S(6,JSF)=  y(3,l) -DELTHETO 


C  End  Boundary  Conditions 

ELSE  IF  (K.GT.K2)  THEN 

C  Enter  non-zero  values: 

S  ( 1 , 7 ) =  CWT 
S(l,8)=  0.0 
S(l,9)=  0.0 
S(l,10)=  -1.0 
S(1,H)=  0.0 
S(l, 12) =  0.0 

S(2,7)=  0.0 
S(2,8)=  CWT 
S(2,9)=  0.0 
S(2,10)=  0.0 
S(2, 11)=  -1.0 
S(2,12)=  0.0 

S(3,7)=  0.0 
S(3,8)=  0.0 
S(3,9)=  CWT 
S(3,10)=  0.0 
S(3,ll)=  0.0 
S(3,12)=  -1.0 

C  Final  values  in  right  hand  vector  for  final  block 
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S(1,JSF)=  Y(1,M)*CWT  -  Y(4,M) 
S(2,JSF)=  Y(2,M)*CWT  -  Y(5,M) 
S(3,JSF)=  Y(3,M)*CWT  -  Y(6,M) 


Interior  Points 

Derived  from  Finite  Difference  Equations  of  Motion 


ELSE 

C  Pre-calculation  of  commonly  used  variables: 

Y1=(Y(1,K)+Y(1,K-1) )/2.0 
Y2=(Y(2.K)+Y(2,K-l))/2.0 
Y3=(Y(3,K)+Y(3,K-l))/2.0 
Y4=(Y(4,K)*Y(4,K-1)  )/2.0 
Y5=(Y(5,K)*Y(5,K-1) ) /2.0 
Y6=(Y(6,K)+Y(6,K-1) )/2.0 

CTD=COS<THETAD) 

STD=SIN(THETAD) 

CTDY3=C0S (THETAD-Y3 ) 

STDY3=SIN (THETAD-Y3 ) 

P=  Y2*CTD-Y1*STD 

IF(ABS(Y3) .GT.EPS)THEN 
FY3=SIN(Y3) / (Y3) 

FPY3=(COS(Y3)/Y3)-(SIN(Y3)/(Y3**2)  ) 
SlG4=FPY3/2.0 

SIG12=(-SIN(Y3)/Y3-2.0*COS(Y3) / (Y3) **2  ♦ 
k  2.0*SIN(Y3)/(Y3)**3)/2.0 

ELSE 

FY3=1.0 

FPY3=0.0 

SIG4:0.0 

SIG12=-l/6.0 

ENDIF 

U1=Y4*CTDY3  ♦  Y5*STDY3  ♦  Y6*P*FY3 

SIG1=  -Y6*FY3 •STD/2 .0 
SIG2=  Y6*FY3*CTD/2 .0 

SIG3=  Y4/2.0*STDY3  -  Y5/2.0*CTDY3  ♦  Y6*P*SIG4 


SIG6=  CTDY3/2.0 

SIG8=  STDY3/2.0 
SIG9=  P*FY3/2.0 
SIGIO  =  -STD/2.0 
SIG11=  CTD/2.0 


c  Enter  non-zero  values: 

S(l,l)=  -1  ♦  H*CTDY3*SIG1 
S(l,2)=  H*CTDY3*SIG2 
S(l,3)=  H* (CTDY3*SIG3  •  STDY3*01/2 . 0) 
S(l,4)=  H*(CTDY3*SIG6) 

S(l,5)=  H*(CTDY3*SIG8) 

S(l,6)=  H*CTDY3*SIG9 
S(l,7)=  S(l,l)  2.0 
S(l,8)=  S(1.2) 

S(l,9)=  S(l,3) 

S(1.10)=  S(l,4) 

S(l, 11)=  S(l,5) 

S(l,12)=  S(l,6) 

S(2, 1)=  H*STDY3*SIG1 
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S(2,2)=  -1  *  H*STDY3*SIG2 

S(2,3)=  H* (STDY3*SIG3  -  CTDY3*Ul/2 .0) 

S(2,4)=  H* (STDy3*SIG6) 

S(2,5)=  H* (STDY3*SIG8) 

S(2,6)=  H*STDY3*SIG9 
S|2,7)=  S(2,l) 

S(2,8)=  S(2,2)  +  2.0 
S{2,9)=  S(2,3) 

S(2,10)=  S(2,4) 

S(2,ll)=  S(2,5) 

S(2,12)=  S(2,6) 

S(3,l)=  H*FY3* (P*SIG1+SIG10*U1) 

S(3,2)=  H*FY3* (P*SIG2+SIG11*U1) 

S(3,3)=  -1  ♦  H*(ALPHA/2.0  »  P*(01*SIG4  *  SIG3*FY3)) 

S(3.4)=  H*P*FY3*SIG6 

S(3,5)=  H*P*FY3*SIG8 

S(3,6)=  H*P*FY3*SIG9 

S(3.7)=  S(3,l) 

S(3,8)=  S(3,2) 

S(3,9)=  S(3,3)  +  2.0 
S(3,10)=  S(3,4) 

S(3,ll)=  S(3,5) 

S(3,12)=  S(3,6) 

S(4,l)=  H*Y6*STD*FY3*SIG1 
S(4,2)=  H*Y6*STD*FY3*SIG2 
S(4,3)=  H*Y6*STD* (01*SIG4  +  SIG3*FY3) 

3(4,4)=  -1  ♦  H*Y6*STD*FY3*SIG6 
3(4,5)=  H*Y6*3TD*FY3*3IG8 
3(4,6)=  H*3TD*FY3* (Y6*3IG9  ♦  Ul/2.0) 

3(4,7)=  3(4,1) 

3(4,8)=  3(4,2) 

3(4,9)=  3(4,3) 

3(4,10)=  3(4,4)  4-  2.0 
3(4,11)=  3(4,5) 

3(4,12)=  3(4,6) 

3(5,1)=  -H*Y6*CTD*FY3*SIG1 
3(5,2)=  -H*Y6*CTD*FY3*SIG2 
3(5,3)=  -H*Y6*<rrD*(Ul*SIG4  ♦  SIG3TY3) 

3(5,4)=  -H*Y6*CTD*3IG6*FY3 
3(5,5)=  -1  -  H*Y6*CTD*3IG8*FY3 
3(5,6)=  -H*CTD*FY3* (Y6*SIG9  ♦  Ul/2.0) 

3(5,7)=  3(5,1) 

3(5,8)=  3(5,2) 

3(5,9)=  3(5.3) 

3(5,10)=  3(5,4) 

3(5,11)=  3(5,5)  ♦  2.0 
3(5,12)=  3(5,6) 

3(6,1)=  -H* (Y4*STDY3*3IG1  -  Y5*CTDY3*SIG1  ♦ 

&  Y6*FPY3*(P*3IG1*SIG10*U1) ) 

3(6,2)=  -H* (Y4*3TDY3*SIG2  -  Y5*CTDY3*3IG2  * 

&  Y6*FPY3*  (P*SIG2-43IG11*U1)  ) 

3(6,3)=  -H*(Y4*(-Ul*CTDY3/2.0  +  3IG3*3TDY3) 

-  Y5* (Ul*3TDY3/2 .0  *  CTDY3*SIG3) 

♦  Y6*P* (U1*3IG12  *  SIG3*FPY3)) 

3(6,4)=  -H* (3TDY3* (Y4*3IG6  *  Ul/2.0)  -  Y5*CTDY3*SIG6  ♦ 

&  Y6*P*FPY3*SIG6) 

3(6,5)=  -H* (Y4*3TDY3*3IG8  -  CTDY3* (Y5*SIG8  *  Ul/2.0)  ♦ 
t  Y6*P*FPY3*SIG8) 

3(6,6)=  -1  -H* (Y4*3TDY3*SIG9  -  Y5‘CTDY3*S1G9 
(,  ♦Y6*P*FPY3*3IG9  *  ( ALPHA*P*U1*FPY3  ) /2 . 0 ) 

3(6,7)=  3(6,1) 

3(6,8)=  3(6,2) 

3(6,9)=  3(6,3) 

3(6,10)=  3(6,4) 

3(6,11)=  3(6,5) 
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S(6,12)=  S(6,6)  +  2.0 


& 


S(l, JSF) =  Y(l. K) -Y(1,K-1) ♦H*(CTDY3*U1) 

S(2, JSF)=  Y(2,K)-Y(2,K-1)+H*(STDY3*U1) 

S(3,  JSF)  =  Y(3,  K)  -  Y(3,K-1)-*H*  (ALPHA*Y3*P*U1  *FY3  ) 

S(4, JSF) =  Y(4,K)-Y(4,K-1)*H*Y6*U1*STD*FY3 
S(5, JSF) =  Y(5, K) -Y(5,K-1) -H*Y6*U1*CTD*FY3 
S(6, JSF) =  Y(6, K) -Y(6,K-1) -H* (Y4*U1*STDY3-Y5*U1*CTDY3* 
Y6* (ALPHA»P*U1*FPY3) ) 


ENDIF 


Dummy  use  of  variables  to  prevent  inocculous  warning  on  MS  Compiler 
(Variables  not  used) 

ISl  =  ISl 
ISF  =  ISF 

INDE3CV(1)  =  INDEXV(l) 

NE  =  NE 


RETURN 

END 


116 


SUBROUTINE  XLATOR2 ( Y , YDOT , H , NY J , NYK , X , THETAD , EPS , ALPHA , 
&  XO,  YO,THETAO,XF,  YF.POS) 


MODIFIED  7/21/94  FOR  FIXED  VIRTUAL  (TARGET)  ROBOT  PROBLEM 
SUCH  THAT  UD=0  FOR  ALL  TIME 


IMPLICIT  REAL’S  (A-H,  0-2) 

PARAMETER (M=201) 

DIMENSION  Y( NY J, NYK) ,  YDOT(5,M),  X(M),  P0S(5,M), 
&  COST  (M)  ,  NRG  (M)  ,U1TRAJ(M) 

POS(1,1)=XO 
POS(2,1)=YO 
POS (3,1) =THETAO 
P0S(4,1)=XF 
POS (5,1) =YF 


NRG(1)=0. 


GU1  =  1 

DO  10  K=1,M 

IF(ABS(Y(3,K) ) .GT.EPS)THEN 
FY3=  SIN(Y(3,K) ) /Y(3,K) 
ELSE 

FY3s  1.0 
ENDIF 


Pdel=  Y ( 2 , K) ’COS (THETAD) - Y ( 1 , K ) *S1N (THETAD) 

Ul=  Y(4,K) •COS(THETAD-Y(3,K) )  ♦ 

S.  Y(5,K)  ♦S1N(THETAD-Y(3,K)  )  ♦ 

&  Y(6,K) ’PdelTYl 

U1TRAJ(K)=U1 

ENERGY  COST  FUCTION  FOR  ROBOT4  ^  2 
NRG(K)=(UlTRAJ(K)’*2)/2 

ENERGY  COST  FUNCTION  IN  FORM  FOR  ROBOTS  (used  for  comparison) 
NRG(K) = (UITRAJ(K) ••2) /2  ♦  ALPHA 

UD=  0.0 


YDOT(l,K)=  COS(THETAD-Y(3,K) ) *U1 
YDOT(2,K)=  SIN(THETAD-Y(3,K))*U1 
YDOT(3,K)=  PdeX*Ul*FY3  ’  ALPHA*Y(3.K) 
YDOT(4,K)=  COS (THETAD) *UD 
YIX)T(5,K)=  SIN  (THETAD)  *UD 


IF(K.GT.  DTHEN 

POS(l,K)=XF-Y(l,K) 

POS(2,K)=YF-Y(2,K) 

POS ( 3 , K) =THETAD-Y { 3  ,  K) 

POS(4,K)=XF 

POS(5,K)=YF 

C 

C  TRAPEZOIDAL  INTEGRATION: 

C 

COST(K) =COST(K-l) +H* (NRG(K) »NRG(K-1) ) /2 
ENDIF 


& 


& 


WRITE(34, 80) 
WRITE(35, 80) 
WRITE(37.81) 


X(K) ,YDOT(l.K) ,YDOT(2,K) ,YDOT(3.K) ,YDOT(4,K) , 
YDOT(5,K) 

X(K)  ,  POS(l,K)  ,POS(2,K)  ,POS(3,K)  ,POS(4.K)  , 
POS(5,K) 

X(K) ,U1TRAJ (K) ,GUl,COST(K) 
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APPENDIX  D 


Program  Files  Specific  to  Robot  3 

R0B0T3 . FOR 

DIFEQ.FOR  {for  Robot  3) 
XLAT0R3 . FOR 
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PROGRAM  ROBOT3 


Single  Robot  with  Terminal  Costs  and  g(ul) =function  of  ul . 

Source  for  subroutines  Red,  Pinvs,  Solvde,  and  Bksub  and  model  for 
Difeq  and  Diskmain: 

Numerical  Recipes,  William  K.  Press,  et  al 


IMPLICIT  REAL’S  (A-H,  0-Z) 


PARAMETER  (NE=6,M=201,MB=3,  NCI  =NE ,  NCJ=NE-NB*  1 ,  NCK=M+ 1 ,  NS  I  =NE , 

&  NSJ=2*NE+1,  NYJ=NE,  NYK=M) 

DIMENSION  SCALV(NE)  ,  INDEXV(NE)  ,  YfNE.M)  ,  C  (NCI ,  NCJ,  NCK)  ,S(NSI,NSJ)  , 
&  YDOT(NE-l,M) ,P0S(NE-1,M) 

COMMON  X(M),  H,  DELXO ,  DELYO ,  DELTHETO,  THETAD,  EPS,  ALPHA,  CWT, 
t.  EPS2 


Variable  description: 
GENERAL  PROGRAM  VARIABLES: 


NE:  Number  of  independent  equations  describing  system 

M:  Number  of  Heshpoints,  divisions  of  independent  variable,  time 

NB:  Number  of  Boundary  Conditions  known  at  initial  condition 

C:  3-D  Array  for  storage  of  corrections  for  each  iteration 

Note:  largest  array  in  program 
NCI,  NCJ,  NCK: 

dimension  variables  of  C  array,  must  satisfy  equations 
found  in  par2uneter  statement 
S:  array  for  storage  of  blocks  of  solution  of  Difeq. 

NSI,  NSJ: 

dimension  variables  of  2-D  S  array,  must  satisfy  equations 
found  in  parameter  statement 

Y:  2-D  array  containing  initial  guess  for  each  point.  This  array 

is  updated  by  calculated  corrections.  When  the  corrections 
are  sufficiently  small,  convergence  is  acheived. 

X;  Array  for  independent  variable,  time.  Used  only  for  comparison 
of  dependent  variables  after  program  completes. 

SCALV:  Array  of  values  representing  the  typical  magnitude  of  the 

dependent  variables.  Used  for  controlling  correction  magnitude. 
INDEXV: lists  column  ordering  for  variables  in  S  array,  not  used  in  this 
program 

ITHAX:  Maximum  number  of  iterations 

CONV:  Convergence  criteria  for  corrections  to  Y 

SLOWC:  Controls  fraction  of  corrections  applied  to  Y 

H:  Increment  of  independent  variable,  divisions  between  mesh  points 

PROBLEM  SPECIFIC  VARIABLES; 


XO; 

YO: 

THETAO : 
XF: 

YF: 

THETAD : 
DELXO : 
DELYO : 
DELTHETO ; 
EPS; 

EPS2: 
ALPHA: 
ALPHAMAX  : 


Initial  X  coordinate  of  robot 
Initial  Y  coordinate  of  robot 

Initial  angle  wrt  X  axis  of  robot 
Desired  final  X  coordinate  of  robot 
Desired  final  Y  coordinate  of  robot 

Desired  final  angle  coordinate  of  robot 
Initial  boundary  condition  for  state  variable  delta-X 
Initial  boundary  condition  for  state  variable  delta-Y 
Initial  boundary  condition  for  state  variable  delta-theta 
Smallest  value  for  which  f (delta-x) =Sin (delta-x) / (delta-x) 
Smallest  value  of  ul  for  which  g(ul)=l. 

Rotational  gain  related  to  delta- theta-dot 

dummy  VARIABLE  USED  FOR  CONTINUITY  WITH  OTHER  FORMS  OF 


o  o 


ROBOT. 


CWT:  Weighting  parameter  for  Terminal  Costs 

RLl ,  RL2 ,  RI,3  :  Initial  guess  amplitude  for  lambda  costates. 

OHMl ,  0HM2 , 0HM3  :  Initial  guess  freqpjencies  for  lambda  costates. 

DC1,DC2,DC3:  Initial  guess  d.c.  offsets  for  lambda  costates. 

PHIl , PHI2 , PHI3 :  Initial  guess  phase  lag  for  lambda  costates. 

SL1,SL2,SL3:  Initial  guess  SCALV,  scale  sizes,  for  lambda  costates. 

P0S(NE-1,M):  Position  Trajectory  for  x,y, theta,  xd  and  yd. 


OPEN (21, FILE= ' xplot . rob3 ' ,  STATUS = 'UNKNOWN' ) 
OPEN(22,FILE=’lplot.rob3' ,  STATUS =' UNKNOWN ' ) 
OPEN(30,FILE='xiplot.rob3' ,  STATUS= ' UNKNOWN ' ) 
OPEN(31,FILE='liplot.rob3' ,  STATOS= ' UNKNOWN ' ) 
OPEN(32,FILE='itplot.rob3' ,  STATUS= ' UNKNOWN ' ) 

OPEN ( 3  3 , FILE= ' tes  tda  t . rob3 ' ,  STATUS= ' UNKNOWN • ) 

OPEN (34 , FILE= 'dotplot . rob3 ' ,  STATUS = 'UNKNOWN' ) 
OPEN(35,FILE='posplot.rob3 ' ,  STATUS= ' UNKNOWN ' ) 
OPEN(37,FILE='ulplot.rob3' ,  STATOS= ' UNKNOWN ' ) 

PI=  3.141592654 

PRINT  *,  'ENTER  ITMAX,  CONV,  SLOWC' 

READ  *,  ITMAX,  CONV,  SLOWC 
PRINT  *, 'ENTER  ALPHA' 

READ  *,  ALPHA 

Dummy  read  for  compatability  with  standard  data  input 
print  *,  'enter  dummy  number' 

READ  * ,  dummy 

PRINT  *,  'ENTER  C,  WEIGHTING  PARAMTER  FOR  TERMINAL  COST' 
READ  »,  CWT 

PRINT  ♦, 'ENTER  EPSILON' 

READ  *  EPS 

PRINT  ♦, 'ENTER  EPSILON  2' 

READ  *  EPS2 

PRINT  *,  'ENTER  INITIAL  X,Y,  AND  THETA(degrees)  ' 

READ  ♦,  XO.YO.THETAO 
THETAO  =THETA0  *  PI / 1 8 0 

PRINT  *,  'ENTER  FINAL  X,Y,  AND  THETA (degrees ) ' 

READ  *,  XF,YF,THETAD 
THETAD=THETAD  •  PI  / 1 8  0 

PRINT  *, 'ENTER  INITIAL  GUESS  FOR  3  LAMBDA  AMPLITUDES' 
READ  *,  RL1,RL2,RL3 

PRINT  *,  'ENTER  INITIAL  GUESS  FOR  3  LAMBDA  FREQUENCIES' 
READ  *,  OHMl ,  OHM2  ,  OHM3 

PRINT  ♦, 'ENTER  INITIAL  GUESS  FOR  3  LAMBDA  DC  OFFSETS' 
READ  ♦,  DC1,DC2,DC3 

PRINT  *, 'ENTER  INITIAL  GUESS  FOR  3  LAMBDA  PHASES' 

READ  *,  PHIl,  PHI2,  PHI3 
PHI1=PHI1*PI/180 
PHI2=PHI2*PI/180 
PHI3=PHI3*PI/180 

PRINT  ♦,  'ENTER  INITIAL  GUESS  FOR  SI2E  OF  3  LAMBDA  VALUES’ 
READ  *,  SL1,SL2,SL3 

DELX0=(XF-X0) 

DELY0=(YF-Y0) 

DELTHET0= (THETAD-THETAO ) 

H=1./(M-1) 

NO  INDEX  CHANGES  NECESSARY 
Index  Scale  used  by  SOLVDE.FOR 

INDEXV(l) =1 
INDEXV(2) =2 
INDEXV(3)=3 


121 


no  nn 


INDEXV(4) =4 
INDEXV(5)=5 
INDEXV(6)=6 

C  INITIAL  GUESS  FOR  ALL  POINTS,  1  -  M 

C  Initialize  independent  vector  X  (time) 

DO  11  K=1,M 

X(K)=(K-1)*H 

11  CONTINUE 

C  INITIAL  GUESS : 

Enter  initial  values  for  all  meshpoints 

NOTE:  BOUNDARY  CONDITONS  FOR  Y{l)-Y(3)  ARE 

ENTERED  AT  POINTS  1  AND  M  DURING  THE  INITIAL  GUESS  •  !  ! 
THESE  NUMBERS  MUST  COINCIDE  WITH  ANY  DESIRED  B.C.  !  !  ! 

WRITE (3 3.*)  'STRAIGHT  LINE  GUESS' 

DO  12  K=1,M 

y(l,K) =DELX0- (DELXO* (K-1) /(M-1) ) 
y(2,K)=DELY0- (DELYO* (K-1) / (M-1) ) 
y ( 3 , K ) =DELTHET0 - ( DELTHETO •(K-1)/(M-1)) 
y (4,K) =RL1*SIN(2*PI*0HM1* (K-1) / (M-1) ♦PHIl) *001 
y(5,K)=RL2*SIN(2*PI*OHM2*(K-l)/(M-l)*PHI2)»DC2 
y(6,K)=RL3*SIN(2*Pl*OHM3‘(K-l)/(M-l)*PHI3)+DC3 

12  CONTINUE 

C  Write  initial  guess  to  file 
DO  13  K=1,M 

WRITE  (30,  80)  X(K)  ,Y(l,K),y(2,K),Y(3,K) 

WRITE (31, 80)  X(K) ,y(4,K) ,Y(5,K) ,Y(6,K) 

13  CONTINUE 

C  Scalv  set  to  approximate  size  of  typical  values  of  )cnown  solution 

SCALV ( 1 ) = ABS ( DELXO / 2 ) ♦ . 0 1 
SCALV  ( 2 )  =ABS  ( DELYO  /  2 )  ♦  .  0 1 
SCALV ( 3 ) =ABS ( 2 ♦DELTHETO /M ) ♦ . 01 
SCALV  ( 4 )  =  ABS  ( SLl )  ♦  .  0 1 
SCALV(5)=ABS(SL2)*.01 
SCALV  ( 6 )  =  ABS  ( SL3  )  4^ .  0 1 

C  WRITE  TEST  DATA  '(TO  FILE 

WRITE(33,*)  'ITMAX  = ' , ITMAX 
WRITE(33,*)  'CONV  =',CONV 
WRITE (33,*)  'SLOWC  =',SLOWC 
WRITE(33,*)  'ALPHA  =', ALPHA 
WRITE(33,*)  'CWT=',CWT 
WRITE(33,*)  'EPSILON  = ' , EPS 
WRITE(33,*)  'EPSILON  2  =',EPS2 

WRITE(33,*)  'INITIAL  X,Y  k  THETA  =  ',  XO , TO , (THETAO* 180/PI ) 
WRITE(33,*)  'FINAL  X,Y  k  THETA  =  ',  XF, YF, (THETAD* 180/PI ) 

WRITE (33,*)  'INITIAL  LAMBDA  VALUES',  RLl , RL2 , RL3 
WRITE (3 3,*)  'SIZE  OF  LAMBDA  VALUES',  SL1,SL2,SL3 
WRITE(33,*)  'DELXO  =',  DELXO 
WRITE(33,*)  'DELYO  =',  DELYO 
WRITE (3 3,*)  'DELTHETO  =',  DELTHETO 

CALL  SOLVDE  ( ITMAX ,  CONV,  SLOWC ,  SCALV,  INDEXV,  NE ,  NB ,  M ,  Y ,  NYJ .  NYK , 

*  C,NCI,NCJ,NCK,S,NSI,NSJ) 

C  Write  final  Y  values  to  file: 

DO  181  )c  =  1,M 

WRITE (21, 80)  X(K)  ,Y(l,)t)  ,Y(2,lc)  ,Y(3,)i) 

WRITE  (22, 80)  X  (K)  ,  Y  (4  ,  )t)  ,  Y  (5 ,  It)  ,  Y  ( 6,  )c) 


122 


181 


CONTINUE 


80  FORMAT(2X,4F15.4) 

CALL  XLATOR3  (Y,  YDOT,  H,  NYJ,  NYK,  X,  THETAD.  EPS .  ALPHA, 
t  XO,YO,THETAO,XF,YP,POS.EPS2) 

PRINT  ♦, 'PROGRAM  COMPLETED' 

CLOSE (21) 

CLOSE (22) 

CLOSE (30) 

CLOSE(31) 

CLOSE (32) 

CLOSE (33) 

CLOSE (34) 

CLOSE (35) 

CLOSE (37) 

END 
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SUBROUTINE  DIFEQ ( K, K1 , K2  ,  JSF, ISl , ISF, INDEXV, NE , S , NSI , NS J , Y , NYJ , 
*  NYK) 

IMPLICIT  REAL*8  (A-H,  0-2) 


*  MODIFIED  7/18/94  TO  INCLUDE  TERMINAL  COST  AT  FINA  BOUNDARY  CONDITION 

*  MODIFIED  7/21/94  FOR  NON-MOVING  VIRTUAL  (TARGET)  ROBOT 

*  MODIFIED  7/25/94  FOR  FUNCTION,  G{U1)  TIMES  ALPHA  TERM  IN  U2 . 

*  (FEEDBACK  REFINEMENT) 

PARAMETER  (M=201) 

DIMENSION  Y(NYJ, NYK)  ,  S(NSI,NSJ),  INDEXV(NYJ) 

COMMON  X(M),  H,  DELXO,  DELYO ,  DELTHETO,  THETAD,  EPS,  ALPHA,  CWT, 
&  EPS2 


C  Initialize  nvatrix  S  as  0 

DO  10  1=1, NSI 

DO  9  J=1,NSJ 

S(I,J)  =  0.0 

9  CONTINUE 

10  CONTINUE 


C  Initial  Boundary  Conditions 

IF(K.EQ.Kl)  THEN 

C  Enter  non-zero  values: 

DO  11  1=  1,3 

S(3*I,6+I)=1.0 
11  CONTINUE 

C  Initial  values  in  right  hand  vector  for  initial  bloc)c 

S(4,JSF)=  y(l,l)-DELX0 
S(5,JSF)=  y(2,l)-DELY0 
S(6,JSF)=  y ( 3, 1) -DELTHETO 


C  End  Boundary  Conditions 

ELSE  IF  (K.GT.K2)  THEN 

C  Enter  non-zero  values: 

S(l,7)=  CWT 
S(l,8)=  0.0 
S(l,9)=  0.0 
S(l,10)=  -1.0 
S(l,ll)=  0.0 
S(l,12)=  0.0 

S(2,7)=  0.0 
S(2,8)=  CWT 
S(2,9)=  0.0 
S(2,10)=  0.0 
S(2,ll)=  -1.0 
S(2,12)=  0.0 

S(3,7)=  0.0 
S(3,8)=  0.0 
S(3,9)=  CWT 
S(3,10)=  0.0 
S(3,ll)=  0.0 
S(3,12)=  -1.0 
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Final  values  in  right  hand  vector  for  final  block 


S(1,JSF)=  Y(1,M)*CWT  -  Y(4,M) 
S(2,JSF)=  Y(2,M)*CWT  -  Y(5,M) 
S(3,JSF)=  Y(3,M)*CWT  -  Y(6,M) 


C  Interior  Points 

C  Derived  from  Finite  Difference  Equations  of  Motion 

ELSE 

C  Pre-calculation  of  commonly  used  variables: 

Y1=(Y(1,K)+Y(1,K-1) )/2.0 
Y2=(Y(2,K)*Y<2.K-1))/2.0 
Y3=(Y(3,K)+Y(3,K-l) )/2.0 
Y4=(Y(4,K)+Y(4,K-1) )/2.0 
Y5={Y(5,K)+Y(5,K-1) )/2.0 
Y6=(Y(6,K)*Y(6,K-1) )/2.0 

CTD=COS (THETAD) 

STD=SIN(THETAD) 

CTDY3=COS  (THETAD-Y3 ) 

STDY3  =SIN (THETAD- Y3 ) 

P=  Y2*(rrD-Yl*STD 

IF(ABS(Y3) .GT.EPS)THEN 
FY3=SIN(Y3) /(Y3) 

FPY3=(COS(Y3) /Y3) - (SIN(Y3) / (Y3**2) ) 

SIG4=FPY3/2.0 

SIG12=(-SIN(Y3)/Y3-2.0‘COS(Y3)/(Y3)**2  + 

&  2.0*SIN(Y3)/{Y3)**3)/2.0 

ELSE 

FY3  =  1 .0 
FPY3=0.0 
SIG4=0.0 
SlG12=-l/6.0 
ENDIF 

U1=Y4*CTDY3  ♦  Y5*STDY3  ♦  Y6»P*FY3 

IF(ABS(U1) .GT.EPS2)THEN 
GU1=1.0 
ELSE 

GUI =0.0 
ENDIF 

SIG1=  -Y6*FY3*STD/2 .0 
SIG2=  Y6*FY3*CTD/2.0 

SIG3=  Y4/2.0*STDY3  -  Y5/2.0*CTDY3  ♦  Y6*P*SIG4 


SIG6=  CTDY3/2.0 

SIG8=  STDY3/2.0 
SIG9=  P*FY3/2.0 
SIGIO  =  -STD/2.0 
SIG11=  CTD/2.0 

c  Enter  non- zero  values: 

S{1,1)=  -1  ♦  H*CTDY3*SIG1 
S(l,2)=  H*CTDY3*SIG2 
S(l,3)=  H* (CTDY3*SIG3  ♦  STDY3*Ul/2 . 0) 
S(l,4)=  H* (CTDY3*SIG6) 

S(l,5)=  H* (CTDY3*SIG8) 

S(l,6)=  H*CTDY3*SIG9 
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S(l,7)=  S(l,l)  +  2.0 
S(l,8)=  S(l,2) 

S(l,9)=  S(l,3) 

S(l,10)=  S(l,4) 

S(l,ll)=  S(l,5) 

S(l,12)=  S(l,6) 

S(2,l)=  H*STDif3*SIGl 

S(2,2)=  -1  +  H*STDY3*SIG2 

S(2,3)=  H* (STDY3*SIG3  -  CTDY3*Ul/2 . 0) 

S(2,4)=  H* (STDY3*SIG6) 

S(2,5)=  H»(STDY3*SIG8) 

S(2,6)=  H*STDY3*SIG9 
S(2,7)=  S(2,l) 

S(2,8)=  S(2,2)  ♦  2.0 
S(2,9)=  S(2,3) 

S(2,10)=  S(2,4) 

S(2,ll)=  S(2,5) 

S(2,12)=  S(2,6) 

S(3,l)=  H*FY3* (P*SIG1*SIG10*U1) 

S(3,2>=  H*FY3* (P*SIG2*SIG11*U1) 

S(3,3)=  -1  *  H*(ALPHA*GUl/2.0  ♦  P*{U1*SIG4  ♦  SIG3*FY3)) 

S(3,4)=  H*P*FY3*SIG6 

S(3,5)=  H*P*FY3*SIG8 

S(3,6)=  H*P*FY3*SIG9 

S(3,7)=  S(3,l) 

S(3,8)=  S(3,2) 

S(3,9)=  S(3,3)  ♦  2.0 
S(3,10)=  S(3,4) 

S(3,ll)=  S(3,5) 

S(3,12)=  S(3,6) 

S(4,l)=  H*Y6*STD*FY3*SIGl 
S(4,2)=  H*Y6*STD*FY3*SIG2 
S(4,3)=  H*Y6*STD*<01*SIG4  ♦  SIG3»FY3) 

S(4,4)=  -1  ♦  H*Y6»STD*FY3*SIG6 
S(4,5)=  H*Y6*STD»FY3*SIG8 
S(4,6)«  H*STD*FY3*<Y6*SIG9  ♦  Ul/2.0) 

S(4,7)=  S(4,l) 

S(4,8)=  S(4,2) 

S(4,9)=  S(4,3) 

S(4,10)=  S(4,4)  ♦  2.0 
S(4,ll)=  S(4,5) 

S(4,12)=  S(4,e) 

S(5,l)=  -H*Y6*CTD*FY3*SIG1 
S(5,2)=  -H*Y6*CTD*FY3*SIG2 
S(5,3)=  -H*Y6*CTD* (U1*SIG4  +  SIG3*FY3) 

S(5,4)=  -H*Y6*CTD*SIG6*FY3 
S(5,5I=  '1  -  H*Y6*CTD*SIG8*FY3 
S(5,6)=  -H*CTD*FY3* (Y6*SIG9  ♦  Ul/2.0) 

S(5,7)=  S(5,l) 

S(5,8)=  S(5,2) 

S(5,9)=  S(5,3) 

S(5,10)=  S(5,4) 

S(5,ll)=  S(5,5)  ♦  2.0 
S(5,12)=  S(5,6) 

S16,l)=  -H* (y4*STDY3*SIGl  -  Y5*CTDY3 ’SIGl  ♦ 

&  Y6»FPY3* (P*S1G1*SIG10*U1) ) 

S(6,2)=  -H* (Y4*STDY3*SIG2  -  Y5*CTDY3*SIG2  ♦ 
i  Y6*FPY3*(P*SIG2*SIG11*U1)) 

S(6.3)=  -H*(Y4*(-Ul*CTDY3/2.0  ♦  SIG3*STDY3) 
i  -  Y5*(Ul»STDY3/2.0  *  CTDY3*SIG3) 

&  ♦  Y6*P* (U1*SIG12  ♦  SIG3*FPY3)) 

S(6,4)=  -H* (STDY3* (Y4*SIG6  »  Ul/2.0)  -  Y5 'CTDYl *5106  ♦ 
&  Y6*P*FPY3*SIG6) 

S(6,5)=  -H* (Y4*STDY3’SIG8  -  CTDY3* (Y5*SrG8  +  Ul/2.0)  + 


126 


n  o 


& 


& 


Y6*P*FPY3*SIG8) 

S(6,6)=  -1  -H* (Y4*STDY3*SIG9  - 
♦Y6*P*FPY3*SIG9 


S(6,7)=  S(6,l) 
S(6,8)=  S(6,2) 
S(6,9)=  S(6,3) 
S(6,10)=  S(6,4) 
S(6,ll)=  S(6,5) 
S(6,12)=  S(6,6) 


2.0 


Y5*CTDY3*SIG9 

♦  (ALPHA*GU1*P*U1*FPY3) /2 .0) 


Sl 


S ( 1 , JSF )=  Y(1,K)-Y(1,K-1)+H* (CTDY3  *01 ) 

S(2, JSF)=  Y(2,K)-Y(2,K-1)*H*(STDY3»U1) 

S ( 3 , JSF) =  y(3,K)-Y(3,K-l)+H* (ALPHA *GU1 •Y3*P'U1 *FY3 ) 
S(4, JSF)=  Y(4,K) -Y(4,K-1)*H*Y6*U1*STD*FY3 
S ( 5 , JSF) =  Y ( 5 , K) -Y ( 5 , K-1 ) -H*Y6*U1*CTD*FY3 
S(6, JSF) =  Y(6,K) -Y(6,K-1) -H* (Y4*U1*STDY3-Y5*U1*CTDY3+ 
Y6* (ALPHA*GU1+P*U1*FPY3) ) 


ENDIF 

Duirany  use  of  variables  Co  prevent  inocculous  warning  on  MS  Compiler 
(Variables  not  used) 

ISl  =  ISl 
ISF  =  ISF 

INDEXV(l)  =  INDEXV(l) 

NE  =  NE 

RETURN 

END 
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SUBROUTINE  XLATOR3  ( Y ,  YDOT ,  H ,  NY J ,  NYK,  X,  THETAD ,  EPS ,  ALPHA , 
&  XO,YO,THETAO,XF,YF,POS,EPS2) 


*  MODIFIED  7/21/94  FOR  FIXED  VIRTUAL  (TARGET)  ROBOT  PROBLEM 

*  SUCH  THAT  UD=0  FOR  ALL  TIME 

*  MODIFIED  7/25/94  FOR  FUNCTION,  G(U1)  TIMES  ALPHA  TERM  IN  U2 . 

*  (FEEDBACK  REFINEMENT) 


IMPLICIT  REAL'8  (A-H,  0-Z) 

PARAMETER (M=201) 

DIMENSION  Y(NYJ, NYK) ,  YDOT(5,M),  X(M),  POS(5,M), 

&  COST(M)  ,NRG(M)  ,U1TRAJ(M)  ,GU1TRAJ(M) 

POS(1,1)=XO 
POS(2,1)=YO 
POS (3,1) =THETAO 
POS(4,l)=XF 
POS(5, 1) =YF 

NRG  ( 1 )  =0  . 

DO  10  K=1,M 

IF(ABS(Y(3,K) ) .GT.EPS)THEN 
FY3=  SIN(Y(3,K) ) /Y(3,K) 

ELSE 

FY3=  1.0 
ENDIF 

Pdel=  Y ( 2, K) 'COS (THETAD) -Y(1,K)*SIN (THETAD) 

Ul=  Y(4,K)*C0<?(THETAD-Y(3,K))  ♦ 

&  Y(5,K)*SIN(THETAD-Y(3,K))  ♦ 

<.  Y(6,K)  •Pdel*FY3 

U1TRAJ(K)=U1 

ENERGY  COST  FUCTION  FOR  ROBOT4  &  2 
NRG(K) =(U1TRAJ(K) •»2) /2 

ENERGY  COST  FUNCTION  IN  FORM  FOR  ROBOTS  (used  for  comparison) 
NRG(K) =(U1TRAJ(K) **2) /2  ♦  ALPHA 


IF(ABS(U1) .GT.EPS2)THEN 
GU1=1.0 
ELSE 

GU1=0.0 

ENDIF 

GUITRAJ(K) =GU1 
UD-  0.0 

YDOT(l,K)=  COS(THETAD-Y(3.K) ) ‘Ul 
YDOT(2,K)=  SIN(THETAD-Y(3.K) ) 'Ul 
YDOT(3,K)=  Pdel*Ul’FY3  ♦  ALPHA*GU1 ‘Y ( 3 , K) 
YDOT(4,K)=  COS (THETAD) ‘UD 
YDOT(5,K)=  SIN (THETAD) 'UD 

IF(K.GT.1)THEN 

POS(l,K) =XF-Y(1,K) 

POS(2,K)  =YF-Y(2,K) 

POS ( 3 , K) =THETAD- Y ( 3  ,  K) 

POS(4,K)  =XF 
POS(5,K)=YF 
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TRAPEZOIDAL  INTEGRATION: 

COST(K) =COST(K-l)*H* (NRG(K) +NRG(K-1) ) /2 
ENDIF 


WRITE (34, 80)  X(K) ,YDOT(l,K) ,YDOT{2,K) .YDOT(3,K) ,YDOT(4,K) , 
&  YDOT(5,K) 

WRITE (35, 80)  X(K) ,POS(l,K) ,POS(2,K) ,POS(3.K) ,POS(4,K) , 

&  POS(5,K) 

WRITE(37,81)  X(K) ,U1TRAJ{K) ,GU1TRAJ(K) ,COST(K) 

10  CONTINUE 

80  FORMAT (2X, 6F20 . 10) 

81  FORMAT ( 2X, 4F20 . 10) 

RETURN 

END 
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APPENDIX  E 


Program  Files  Specific  to  Robot  4 

R0B0T4 . FOR 

DIFEQ.FOR  (for  Robot  4) 
XLAT0R4 . FOR 
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PRCXSRAM  ROBOT4 


Single  Robot  with  terminal  cost  and  g(Ul)  and  alpha  control  (hi/lo) 


Source  for  subroutines  Red,  Pinvs,  Solvde,  and  Bksub  and  model  for 
Oifeq  and  Diskmain: 

Numerical  Recipes,  William  H.  Press,  et  al 


IMPLICIT  REAL'S  (A-H,  0-Z) 


PARAMETER  (NE=6,M  =  201,NB=3,NCI=NE,NCJ=NE-NB<-1,NCK=M*1,NSI=NE, 

&  NSJ=2'NE+l,  NYJ=NE,  NYK=M) 

DIMENSION  SCALV(NE) , INDEXV(NE) , Y(NE,M) , C (NCI , NCJ , NCK) ,S(NSI,NSJ) , 

&  YDOT(NE-l,M) ,P0S{NE-1,M) 

COMMON  X(M),  H,  DELXO,  DELYO,  DELTHETO,  THETAD,  EPS,  CWT, ALPHA (M) , 
&  EPS2,  ALPHAMIN,  ALPHAMAX 


Variable  description; 
GENERAL  PROGRAM  VARIABLES; 


NE;  Number  of  independent  equations  describing  system 
M;  Number  of  Meshpoints,  divisions  of  independent  variable,  time 

NB;  Number  of  Boundary  Conditions  known  at  initial  condition 

C;  3-D  Array  for  storage  of  corrections  for  each  iteration 

Note;  largest  array  in  program 
NCI,  NCJ,  NCK; 

dimension  variables  of  C  array,  must  satisfy  equations 
found  in  parameter  statement 

S;  array  for  storage  of  blocks  of  solution  of  Difeq. 

NSI,  NSJ; 

dimension  variables  of  2-D  S  array,  must  satisfy  equations 
found  in  parameter  statement 

Y;  2-D  array  containing  initial  guess  for  each  point.  This  array 
is  updated  by  calculated  corrections.  When  the  corrections 
are  sufficiently  small,  convergence  is  acheived. 

X;  Array  for  independent  variable,  time.  Used  only  for  comparison 
of  dependent  variables  after  program  completes. 

SCALV;  Array  of  values  representing  the  typical  magnitude  of  the 

dependent  variables.  Used  for  controlling  correction  magnitude. 
INDEXV; lists  column  ordering  for  variables  in  S  array,  not  used  in  this 
program 

ITMAX;  Maximum  number  of  iterations 

CONV;  Convergence  criteria  for  corrections  to  Y 

SLOWC;  Controls  fraction  of  corrections  applied  to  Y 

H;  Increment  of  independent  variable,  divisions  between  mesh  points 

PROBLEM  SPECIFIC  VARIABLES: 


XO: 

YO: 

THETAO : 

XF: 

YF; 

THETAD ; 

DELXO ; 

DELYO : 

DELTHETO ; 

EPS: 

EPS2  : 

ALPHAMIN: 

ALPHAMAX: 

CWT: 

RL1,RL2,RL3; 
OHMl , 0HM2 , 0HM3 


Initial  X  coordinate  of  robot 
Initial  Y  coordinate  of  robot 

Initial  angle  wrt  X  axis  of  robot 
Desired  final  X  coordinate  of  robot 
Desired  final  Y  coordinate  of  robot 

Desired  final  angle  coordinate  of  robot 
Initial  boundary  condition  for  state  variable  delta-X 
Initial  boundary  condition  for  state  variable  delta-Y 
Initial  boundary  condition  for  state  variable  delta-theta 
Smallest  value  for  which  f (delta-x) =Sin(delta-x) / (delta-x) 
Smallest  value  U1  for  which  g(ul)  =  1 
Low  alpha  gain  for  ul  equal  to  0 
High  alpha  gain  for  ul  not  equal  to  0 
Weighting  parameter  for  Terminal  Costs 
Initial  guess  amplitude  for  lambda  costates. 

Initial  guess  frequencies  for  lambda  costates. 
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DC1,DC2,DC3:  Initial  guess  d.c.  offsets  for  lambda  costates. 

PHIl ,  PHI2,  PHI3  :  Initial  guess  phase  lag  for  l2unbda  costates. 

SL1,SL2,SL3:  Initial  guess  SCALV,  scale  sizes,  for  lambda  costates. 

POS(NE-l,M):  Position  Trajectory  for  x,y, theta,  xd  and  yd. 


OPEN(21,FH,E='xplot.rob4'  ,  STATUS=  *  UNKNOWN  •  ) 

OPEN ( 22 , FILE= ' Iplot . rob4 ' ,  STATUS= ' UNKNOWN ■ ) 

OPEN { 30 , FILE= ‘ xiplot . rob4 • ,  STATOS= ' UNKNOWN ' ) 

OPEN { 3 l,FILE=‘l iplot. rob4 ‘ ,  STATUS=' UNKNOWN' ) 
OPEN(32,FILE='itplot.rob4' ,  STATUS= ' UNKNOWN' ) 

OPEN(33, FILE=' testdat .rob4' ,  STATUS= ' UNKNOWN ' ) 
OPEN(34,FILE='dotplot.rob4' ,  STATUS= 'UNKNOWN' ) 
OPEN(35,FrLE='posploC.rob4',  STATUS= ' UNKNOWN ' ) 
OPEN(36,FILE='alplot.rob4',  STA‘rUS=' UNKNOWN'  ) 
OPEN(37,FILE='ulplot.rob4' ,  STATUS= ' UNKNOWN ' ) 

PI=  3.141592654 

PRINT  *,  'ENTER  ITMAX,  CONV,  SLOWC' 

READ  *,  ITMAX,  CONV,  SLOWC 
PRINT  *,  'ENTER  ALPHAMIN' 

READ  *,  ALPHA  MIN 
PRINT  *, 'ENTER  ALPHAMAX' 

READ  *,  ALPHA  MAX 

PRINT  *,  'ENTER  C,  WEIGHTING  PARAMTER  FOR  TERMINAL  COST' 
READ  ♦,  CWT 

PRINT  *, 'ENTER  EPSILON' 

READ  • ,  EPS 

PRINT  *, 'ENTER  EPSILON  2' 

READ  *,  EPS2 

PRINT  *,  'ENTER  INITIAL  X,Y,  AND  THETA {degrees ) ' 

READ  *,  XO,YO,THETAO 
THETAO  =THETA0  *  PI / 1 8  0 

PRINT  *,  'ENTER  FINAL  X,Y,  AND  THETA (degrees ) ' 

READ  ♦,  XF,YF,THETAD 
THETAD=THETAD*  PI / 1 80 

PRINT  • ,  '  ENTER  INITIAL  GUESS  FOR  3  LAMBDA  AMPLITUDES ' 

READ  *,  RL1,RL2,RL3 

PRINT  ♦, 'ENTER  INITIAL  GUESS  FOR  3  LAMBDA  FREQUENCIES' 
READ  *,  OHMl,OHM2,OHM3 

PRINT  ♦, 'ENTER  INITIAL  GUESS  FOR  3  LAMBDA  DC  OFFSETS' 

READ  ♦,  DC1,DC2,DC3 

PRINT  *, 'ENTER  INITIAL  GUESS  FOR  3  LAMBDA  PHASES’ 

READ  *,  PHIl,  PHI2,  PHI3 
PHI1=PHI1*PI/180 
PHI2=PHI2*PI/180 
PHI3=PHI3*PI/180 

PRINT  *, 'ENTER  INITIAL  GUESS  FOR  SIZE  OF  3  LAMBDA  VALUES' 
READ  *,  SL1,SL2,SL3 

DELX0= (XF-XO) 

DELYO=(YF-YO) 

DELTHET0= (THETAD-THETAO ) 

H=1./(M-1) 

NO  INDEX  CHANGES  NECESSARY 
Index  Scale  used  by  SOLVDE.FOR 

INDEXV(l) =1 
INDEXV(2) =2 
1NDEXV(3)=3 
1NDEXV(4) =4 
1NDEXV(5)=5 
1NDEXV(6)=6 
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c  INITIAL  GUESS  FOR  ALL  POINTS,  1  -  M 

C  Initialize  independent  vector  X  (time) 

DO  11  K=1,M 

X(K)  =  (K-1)*H 

11  CONTINUE 

C  INITIAL  GUESS : 

Enter  initial  values  for  all  meshpoints 

NOTE:  BOUNDARY  CONDITONS  FOR  Y(l)-Y(3)  ARE 

ENTERED  AT  POINTS  1  AND  M  DURING  THE  INITIAL  GUESS!  !  ! 
THESE  NUMBERS  MUST  COINCIDE  WITH  ANY  DESIRED  B.C.  I  !  ! 

WRITE(33,*)  ‘STRAIGHT  LINE  GUESS' 

DO  12  K=1,M 

y(l,K)=DELX0- (DELXO* (K-1) / (M-1) ) 
y(2,K)=DELYO-(DELYO*(K-l)/(M-l)) 
y{3,K)=DELTHET0- (DELTHETO* (K-1) / (M-1) ) 
y  (4 ,  K)  =RL1»SIN(2»PI’'0HM1*  (K-1)  /  (M-1)  ♦PHIl)  +DC1 
y (5, K) =RL2*SIN(2*PI*OHM2* (K-1) / (M-1)»PHI2) +DC2 
y(6, K) =RL3*SIN(2*PI*OHM3* (K-1) / (M-1) +PH13 ) +DC3 

12  CONTINUE 


INITIAL  ALPHA  DETERMINATION: 


IF(Y(3,1) .GT.EPS)  THEN 

FY3=  SIN(Y(3, 1) ) / )Y(3, 1) ) 

ELSE 

FY3=  1.0 
ENDIF 

U10=Y(4, 1) *COS(THETAD-Y(3,l) )  +  Y (5 , 1 ) ‘SIN (THETAD- Y ( 3 , 1 ) ) 
t  ♦  Y(6, 1)  •  (Y(2, 1)  ♦COS(THETAD) -Yd,  1)  •SIN(THETAD)  )  •FY3 

IP(ABS(U10) .GT.EPS2)THEN 
GU1=1.0 
ELSE 

GU1=0.0 

ENDIF 

BETA=1-Y(6,1) •G01*Y(3. 1) 

IF(BETA.GT. (0.0) )THEN 
ALPHA ( 1 ) =ALPHAMIN 

ELSE 

ALPHA ( 1 ) =ALPHAMAX 
ENDIF 


C  Write  initial  guess  to  file 
DO  13  K=1,M 

WRITE (30, 80)  X(K) ,Y(1,K) ,Y(2,K) ,Y(3,K) 

WRITE (31, 80)  X(K)  ,Y(4,K)  ,  Y(5,K)  ,Y(6,K) 

13  CONTINUE 

C  Scalv  set  to  approximate  size  of  typical  values  of  Known  solution 

SCALV ( 1 ) =ABS ( DELXO / 2 ) ♦ . 0 1 
SCALV(2) =ABS!DELY0/2) + .01 
SCALV ( 3 ) = ABS ( 2 ‘DELTHETO / M ) ♦ . 0 1 
SCALV ( 4 ) =ABS (SLl ) ♦ . 01 
SCALV  ( 5 )  =ABS  ( SL2  )  ♦  .  0 1 
SCALV ( 6 ) =ABS ( SL3 ) + . 01 

C  WRITE  TEST  DATA  TO  FILE 

WRITE (3 3,*)  ‘ITMAX  = ' , ITMAX 
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WRITE(33,*)  'CONV  =',CONV 
WRITE(33,*)  'SLOWC  =',SLOWC 
WRITE(33,*)  ‘ALPHA  MIN  =',ALPHAMIN 
WRITE (33,*)  'ALPHA  MAX  =',ALPHAMAX 
WRITE (33,*)  'CWT  =',CWT 
WRITE (33,*)  ‘EPSILON  = ‘ , EPS 
WRITE (33,*)  ‘EPSILON  2  =‘,EPS2 

WRITE(33,*)  ‘INITIAL  X,Y  &  THETA  =  ‘,  XO , YO , (THETAO* 180/PI ) 
WRITE(33,*)  ‘FINAL  X,Y  {.  THETA  =  ‘,  XF,  YF,  (THETAD*180/PI ) 
WRITE(33,*)  ‘ INITIAL  LAMBDA  VALUE  AMPLITUDE' ,  RL1,RL2,RL3 
WRITE (3 3,*)  ‘SIZE  OF  LAMBDA  VALUES',  SL1,SL2,SL3 
WRITE(33,*)  ‘DELXO  =‘,  DELXO 
WRITE(33,*)  ‘DELYO  =‘,  DELYO 
WRITE (3 3,*)  ‘DELTHETO  =‘,  DELTHETO 

CALL  SOLVDE  ( ITMAX ,  CONV ,  SLOWC ,  SCALV ,  INDEXV ,  NE ,  NB ,  M ,  Y ,  NY  J ,  NYK , 
*  C,NCI,NCJ,NCK,S,NSI,NSJ) 

C  Write  final  Y  values  to  file: 

DO  181  )c  =  1,M 

WRITE (21, 80)  X(K)  ,Y(l,)c)  ,Y(2,)c)  ,Y(3,)c) 

WRITE (22, 80)  X (K)  ,  Y (4 , (c)  ,  Y (5, )c)  ,  Y (6 ,  )c) 

WRITE(36,81)  X)K) , ALPHA(K) 

181  CONTINUE 

80  FORMAT(2X,4F15.4) 

81  FORMAT (2X,2F15. 4) 

CALL  XLATOR4 (Y, YDOT, H, NYJ, NYK, X, THETAD, EPS, ALPHA, 
t  X0,YO,THETA0,XF,YF,POS,EPS2) 

PRINT  *, ‘PROGRAM  COMPLETED' 

CLOSE(21) 

CLOSE (22) 

CLOSE (30) 

CLOSE (31) 

CLOSE (32) 

CLOSE (33) 

CLOSE (34) 

CLOSE (35) 

CLOSE (36) 

CLOSE (37) 

END 
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SUBROUTINE  DIFEQ (K, K1 . K2 , JSF, ISl , ISF, INDEXV, NE, S, NSI , NSJ , Y, NYJ, 
*  NYK) 

IMPLICIT  REAL’S  (A-H,  0-Z) 


*  MODIFIED  7/18/94  TO  INCLUDE  TERMINAL  COST  AT  FINA  BOUNDARY  CONDITION 

*  MODIFIED  7/21/94  FOR  NON-MOVING  VIRTUAL  (TARGET)  ROBOT 

*  MODIFIED  7/25/94  FOR  FUNCTION,  G(U1)  TIMES  ALPHA  TERM  IN  U2  . 

*  (FEEDBACK  REFINEMENT) 

*  MODIFIED  7/26/94  FOR  ALPHA  AS  INPUT 


PARAMETER (M=201) 

DIMENSION  Y( NYJ, NYK) ,  S(NSI,NSJ),  INDEXV(NYJ) 

COMMON  X(M),  H,  DELXO,  DELYO,  DELTHETO,  THETAD,  EPS,  CWT, ALPHA (M) , 
k  EPS2,  ALPHAMIN.  ALPHAMAX 


C  Initialize  matrix  S  as  0 

DO  10  1=1, NSI 

DO  9  J=1,NSJ 

S(I,J)  =  0.0 

9  CONTINUE 

10  CONTINUE 


C  Initial  Boundary  Conditions 

IF(K.EQ.Kl)  THEN 
C  Enter  non-zero  values; 


DO  11  1=  1,3 

S(3+I,6’I)=1.0 
11  CONTINUE 

C  Initial  values  in  right  hand  vector  for  initial  block 

S(4,JSF)=  y(l,l)-DELX0 
S(5,JSF)=  y(2,l)-DELY0 
S(6,JSF)=  y ( 3, 1) -DELTHETO 


C  End  Boundary  Conditions 

ELSE  IF  (K.GT.K2)  THEN 

C  Enter  non-zero  values: 

S(l,7)=  CWT 
S(l,8)=  0.0 
S(l,9)=  0.0 
S(l,10)=  -1.0 
S(l,ll)=  0.0 
S(l,12)=  0.0 

S(2,7)=  0.0 
S(2,8)=  CWT 
S(2,9)=  0.0 
S(2,10)=  0.0 
S(2,ll)=  -1.0 
S(2,12)=  0.0 

S(3,7)=  0.0 
S(3,8)=  0.0 
S(3,9)=  CWT 
S(3,10)=  0.0 
S(3,ll)=  0.0 
S(3,12)=  -1.0 
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Final  values  in  right  hand  vector  for  final  block 


S(1,JSF)=  y(l,M)*CWT  -  Y(4,M) 
St2,JSF)=  Y(2,M)*CVn’  -  Y(5,M) 
S(3,JSF)=  Y(3,M)*CWT  -  Y(6,M) 


Interior  Points 

Derived  from  Finite  Difference  Equations  of  Motion 


ELSE 

C  Pre-calculation  of  commonly  used  variables; 

Y1=(Y(1,K)+Y(1,K-1) ) /2.0 
Y2=(Y(2,K)+Y(2,K-l))/2.0 
Y3=(Y(3,K)+Y(3,K-l))/2.0 
Y4=(Y(4,K)+Y(4,K-l))/2.0 
Y5=(Y(5,K)+Y(5,K-l))/2.0 
Y6=(Y(6,K)+Y(6,K-1) ) /2.0 

CTD=COS  (THETAD) 

STD=SIN(THETAD) 

CTDY3=C0S  <THETAD-Y3 ) 

STDY3=SIN(THETAD-Y3) 

P=  Y2*CTD-Y1*STD 

IF(ABS(Y3) .GT.EPSjTHEN 
FY3=SIN(Y3) / (Y3) 

FPY3=(COS(Y3) /Y3) - (SIN(Y3) / (Y3*»2) ) 
SIG4=FPY3/2.0 

SIG12=(-SIN(Y3)  /Y3-2 .0*COS(Y3)/(Y3)**2 
6  2.0*SIN(Y3)/(Y3)**3)/2.0 

ELSE 

FY3=1.0 

FPY3=0.0 

SIG4=0.0 

SIG12=-l/6.0 

ENDIF 

U1=Y4*CTDY3  ♦  Y5*STDY3  ♦  Y5*P*FY3 

IF(ABS(U1) .GT.EPS2)THEN 
GU1=1.0 
ELSE 

GU1=0.0 

ENDIF 

BETA=1-Y6*GU1*Y3 
IF(BETA.GT. (0.0) )THEN 
ALPHA ( K) =ALPHAMIN 

ELSE 

ALPHA ( K ) =ALPHAMAX 
ENDIF 

SIG1=  -Y6*FY3*STD/2 .0 
SIG2=  Y6*FY3*CTD/2 .0 

SIG3=  Y4/2.0*STDY3  -  Y5/2.0*CTDY3  ♦  y6*P*SIC4 


SIG6=  CTDY3/2.0 

SIG8=  STDY3/2.0 
SIG9=  P*FY3/2.0 
SIGIO  =  -STD/2.0 
SIGll=  CTD/2.0 

c  Enter  non-zero  values; 
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S(l,l)=  -1  +  H*CTDY3*SIG1 
S(1.2)=  H*CTDy3*SIG2 
S(l,3)=  H*(CTDY3*SIG3  +  STDY3*Ul/2 .0) 

S(l,4)=  H*(CTDY3*SIG6) 

S(l,5)=  H* (CTDY3*SIG8) 

S(l,6)=  H*CTDY3*SIG9 
S(l,7)=  S{1,1)  +  2.0 
S(l,8)=  S(l,2) 

S(l,9)=  S<1,3) 

S(l,10)=  S(l,4) 

S(l,ll)=  S(l,5) 

S(l,12)=  S(l,6) 

S(2,l)=  H*STDY3*SIG1 

S(2,2)=  -1  +  H*STDY3*SIG2 

S<2,3)=  H»(STDY3*SIG3  -  CTDY3*Ul/2 . 0) 

S(2,4)=  H* (STDY3*SIG6) 

3(2,51=  H*(STDY3*SIG8) 

S(2,6)=  H*STDY3*SIG9 
3(2,7)=  3(2,1) 

3(2,8)=  3(2,2)  +  2.0 
3(2,9)=  3(2,3) 

3(2,10)=  3(2,4) 

3(2,11)=  3(2,5) 

3(2,12)=  3(2,6) 

3(3,1)=  H*FY3* (P*3IG1+3IG10*U1) 

3(3,2)=  H*Fy3*(P*3IG2i-SIGll*01) 

3(3,3)=  -1  ♦  H* (ALPHA(K) •GUl/2.0  ♦  P*(01*3IG4  ♦  SIG3*FY3)) 

3(3,4)=  H*P*FY3*3IG6 

3(3,5)=  H*P*FY3*3IG8 

3(3,6)=  H*P*FY3*3IG9 

3(3,7)=  3(3,1) 

3(3,8)=  3(3,2) 

3(3,9)=  3(3,3)  +  2.0 
3(3,10)=  3(3,4) 

3(3,11)=  3(3,5) 

3(3,12)=  3(3,6) 

3(4,1)=  H*Y6*3TD*FY3*SIG1 
3(4,2)=  H*y6*STD»Fy3*SIG2 
3(4,3)=  H*Y6*STD*(U1*SIG4  ♦  3IG3*FY3) 

3(4,4)=  -1  ♦  H*Y6*STD*FY3*SIG6 
3(4,5)=  H*Y6*3TD*FY3*SIG8 
3(4,6)=  H*3TD*FY3* (Y6*3IG9  ♦  Ul/2.0) 

3(4,7)=  3(4,1) 

3(4,8)=  3(4,2) 

3(4,9)=  3(4,3) 

3(4,10)=  3(4,4)  +  2.0 
3(4,11)=  3(4,5) 

3(4,12)=  3(4,6) 

3(5,1)=  -H*Y6*CTD*FY3*SIG1 
3(5,2)=  -H*y6*CTD*FY3*3IG2 
3(5,3)=  -H*Y6*CTD*(U1*3IG4  ♦  SIG3*FY3) 

3(5,4)=  -H*Y6*CTD*3IG6*FY3 
3(5,5)=  -1  -  H*Y6*CTD*3IG8*FY3 
3(5,6)=  -H*CrrD*FY3*  (y6*SIG9  ♦  Ul/2.0) 

3(5,7)=  3(5,1) 

3(5,8)=  3(5,2) 

3(5,9)=  3(5,3) 

3(5,10)=  3(5,4) 

3(5,11)=  3(5,5)  ♦  2.0 
3(5,12)=  3(5,6) 

3(6,1)=  -H* (y4*3TDY3*3IGl  -  Y5*CTDY3*SIG1  * 

&  y6*FPY3* (P*3IG1+3IG10*U1) ) 

3(6,2)=  -H* (Y4*3TDy3*SIG2  -  Y5*CTDY3*3IG2  » 

&  Y6*FPY3* (P*3IG2*31G11*U1) ) 
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S(6,3)=  -H»(Y4*(-Ul*CTDY3/2.0  ♦  SIG3*STDY3) 

-  Y5*(Ul*STDY3/2.0  +  CTDY3*SIG3) 

+  Y6*P*(U1*SIG12  ♦  SIG3*FPY3)) 

S(6,4)=  -H* (STDY3* (Y4*SIG6  ♦  Ul/2.0)  -  Y5*CTDY3*SIG6  + 
Y6*P*FPY3*SIG6) 

S(6,5)=  -H* (Y4*STDY3*SIG8  -  CTDY3* (Y5*SIG8  ♦  Ul/2.0)  + 
Y6*P*FPY3*SIG8) 

S(6,6)=  -1  -H* (Y4*STDY3*SIG9  -  Y5*CTDY3*SIG9 
+Y6*P*FPY3*SIG9 
+ (ALPHA(K) •GU1+P*U1-FPY3) /2 .0) 

S(6,7)=  S(6.1) 

S(6,8)=  S(6,2) 

S(6,9)=  S(6,3) 

S(6,10)-  S(6,4) 

S(6,ll)=  S(6,S) 

S(6,12)=  S(6,6)  +  2.0 


Si 


S(l, JSF)=  Y(l,K)-Y(l,K-l)+H*(CrDy3*01) 

S(2, JSF)=  Y(2,K) -Y(2,K-1)+H*(STDY3*U1) 

S(3, JSF)=  Y(3,K)-Y(3,K-1)+H*(ALPHA(K) *GU1 *Y3*P*U1*FY3 ) 
S(4, JSF)=  Y(4,K) -Y(4,K-1)»H*Y6*U1*STD*FY3 
S(5, JSF)=  Y(5,K) -Y(5,K-1) -H*Y6*U1*CTD*FY3 
S(6, JSF) =  Y<6,K) -Y(6,K-1) -H»(Y4*U1*STDY3-Y5*U1*CTDY3+ 
Y6* (ALPHA(K) *GU1*P*01*FPY3 ) ) 


ENDIF 

Dunony  use  of  variables  Co  prevent  inocculous  warning  on  MS  Compiler 
(Variables  not  used) 

ISl  =  ISl 
ISF  =  ISF 

INDEXV(l)  =  INDEXV(l) 

NE  s  NE 

RETURN 

END 
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SUBROUTINE  XLATOR4  ( Y ,  YDOT ,  H ,  NY J .  NYK ,  X ,  THETAD ,  EPS ,  ALPHA , 
&  XO,YO,THETAO,XF,YF,POS,EPS2) 


MODIFIED  7/21/94  FOR  FIXED  VIRTUAL  (TARGET)  ROBOT  PROBLEM 
SUCH  THAT  UD=0  FOR  ALL  TIME 

MODIFIED  7/25/94  FOR  FUNCTION,  G(U1)  TIMES  ALPHA  TERM  IN  U2 . 
(FEEDBACK  REFINEMENT) 

MODIFIED  7/26/94  FOR  ALPHA  AS  INPUT,  ALPHAMIN  OR  ALPHAMAX. 


IMPLICIT  REAL'S  (A-H,  0-Z) 

PARAMETER (M=201) 

DIMENSION  Y(NYJ, NYK) ,  YD0T(5,M),  X(M) ,  POS ( 5 , M) , ALPHA (M) , 
&  COST(M)  ,MRG{M)  ,U1TRAJ(M)  ,GU1TRAJ(M) 

POS (1,1) =X0 
POS(2,1)=YO 
POS(3,1)=THETAO 
POS(4, 1) =XF 
POS (5,1) =yF 

NRG ( 1 ) =0 

DO  10  K=1,M 

IF(ABS(y(:‘,K)  )  .GT.EPS)THEN 
FY3=  S1N(Y(3,K)  )  /Y(3,K) 

ELSE 

FY3=  1.0 
ENDIF 

Pdel=  Y ( 2 , K) 'COS (THETAD) - Y ( 1 , K) 'SIN (THETAD) 

Ul=  Y(4,K) *COS(THETAD-Y(3,K) )  ' 

(.  Y(5,K)  *SIN(THETAD-Y(3,K) )  ♦ 

&  Y(6,K) 'Pdel'FYS 

UITRAJ(K) =U1 

ENERGY  COST  FUCTION  FOR  ROBOTS 
NRG(K)  =  (UlTRAJ(K)"2)/2  +  ALPHA(K) 

IF(ABS(U1) . GT . EPS2 ) THEN 
GU1=1 ,0 
ELSE 

GUI =0.0 
ENDIF 


GUITRAJ(K) =GU1 
UD=  0.0 


YDOT(l,K)  = 
YDOT (2,  K)  = 
YDOT(3,K) = 
YDOT(4,K)= 
YDOT(5,K)  = 


COS (THETAD-Y ( 3 , K) ) *U1 
SIN(THETAD-Y(3,K) ) 'Ul 
Pdel*Ul*FY3  ♦  ALPHAOc)  *GU1*Y(3,K) 
COS  (THETAD)  *UD 
SIN (THETAD) 'UD 


IF(K.GT.  DTHEN 

POS(l,K)=XF-Y(l,K) 

POS(2,K)=YF-Y(2,K) 

POS ( 3 , K) =THETAD-Y ( 3 , K) 
POS(4,K)=XF 
POS(5, K) =YF 
C 

C  TRAPEZOIDAL  INTEGRATION: 

C 
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APPENDIX  F 


Program  Files  Specific  to  Robot  5 

ROBOTS . FOR 

DIFEQ.FOR  (for  Robot  5) 
XLAT0R5 . FOR 
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PROGRAM  ROBOTS 


SINGLE  ROBOT  WITH  TERMINAL  COST,  g{Ul»,  AND  PROPORTIONAL  ALPHA  CONTROL 


Source  for  subroutines  Red,  Pinvs,  Solvde,  and  BJcsub  and  model  for 
Difeq  and  Diskmain; 

Numerical  Recipes,  William  H.  Press,  et  al 


IMPLICIT  REAL'S  (A-H,  0-Z) 


PARAMETER  (NE= 6  ,  M=201 ,  NB  =  3  ,  NCI=NE,  NCJ=NE-NB+1 ,  NCK=M+1 ,  NSI=NE, 

&  NSJ=2*NE*1,  NYJ=NE,  NYK=M) 

DIMENSION  SCALV(NE) , INDEXV(NE) , Y (NE, M) , C (NCI , NCJ, NCR) ,S(NSI,NSJ) , 

&  YDOT(NE-l,M) ,POS(NE-l,M) ,01TRAJ(M) 

COMMON  X(M),  H,  DELXO,  DELYO,  DELTHETO,  THETAD,  EPS,  CWT,  ALPHA  (M)  , 
&  EPS2,  ALPHAMIN 


Variable  description; 
GENERAL  PROGRAM  VARIABLES; 


NE;  Number  of  independent  equations  describing  system 
M;  Number  of  Meshpoints,  divisions  of  independent  variable,  time 

NB;  Number  of  Boundary  Conditions  known  at  initial  condition 

C;  3*D  Array  for  storage  of  corrections  for  each  iteration 

Note;  largest  array  in  program 
NCI,  NCJ,  NCR; 

dimension  variables  of  C  array,  must  satisfy  equations 
found  in  parameter  statement 
S;  array  for  storage  of  blocks  of  solution  of  Difeq. 

NSI,  NSJ; 

dimension  variables  of  2-D  S  array,  must  satisfy  equations 
found  in  parameter  statement 

Y;  2-D  array  containing  initial  guess  for  each  point.  This  array 

is  updated  by  calculated  corrections.  When  the  corrections 
are  sufficiently  small,  convergence  is  acheived. 

X;  Array  for  independent  variable,  time.  Used  only  for  comparison 
of  dependent  variables  after  program  completes. 

SCALV :  Array  of  values  representing  the  typical  magnitude  of  the 

dependent  variables.  Used  for  controlling  correction  magnitude. 
INDEXV; lists  column  ordering  for  variables  in  S  array,  not  used  in  this 
program 

ITMAX;  Maximum  number  of  iterations 

CONV;  Convergence  criteria  for  corrections  to  Y 

SLOWC;  Controls  fraction  of  corrections  applied  to  Y 

H;  Increment  of  independent  variable,  divisions  between  mesh  points 

PROBLEM  SPECIFIC  VARIABLES; 


•  XO; 

•  YO; 

•  THETAO ; 

•  XF; 

•  YF; 

'  THETAD; 

•  DELXO ; 

•  DELYO ; 

•  DELTHETO ; 

•  EPS: 

•  EPS2 : 

•  ALPHA: 

•  ALPHAMAX : 
ROBOT. 


Initial  X  coordinate  of  robot 
Initial  Y  coordinate  of  robot 

Initial  angle  wrt  X  axis  of  robot 
Desired  final  X  coordinate  of  robot 
Desired  final  Y  coordinate  of  robot 

Desired  final  angle  coordinate  of  robot 
Initial  boundary  condition  for  state  variable  delta-X 
Initial  boundary  condition  for  state  variable  delta-Y 
Initial  boundary  condition  for  state  variable  delta-theta 
Smallest  value  for  which  f (delta-x) =Sin(delta-x) / (delta-x) 
Smallest  value  of  U1  for  which  g(Ul)=l 
Rotational  gain  related  to  delta-theta-dot 
DUMMY  VARIABLE  USED  FOR  CONTINUITY  WITH  OTHER  FORMS  OF 
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CWT:  Weighting  parameter  for  Terminal  Costs 

RIil,RL2,RL3;  Initial  guess  amplitude  for  launbda  costates. 
OHMl,OHM2,OHM3 :  Initial  guess  frequencies  for  lambda  costates. 

DCl,OC2,OC3:  Initial  guess  d.c.  offsets  for  lambda  costates. 

PHIl , PHI2, PHI3 :  Initial  guess  phase  lag  for  lambda  costates. 

SL1,SL2,SL3;  Initial  guess  SCALV,  scale  sizes,  for  Izunbda  costates. 
P0S{KE-1,M):  Position  Trajectory  for  x,y, theta,  xd  and  yd. 


OPEN (21, FILE= ‘ xplot . rob5 ' ,  STATUS=  *  UNKNOWN' ) 
OPEN(22,FILE=' IplOt.robS' ,  STATUS =' UNKNOWN' ) 

OPEN ( 3 0 , FILE= ' xiplot . r obS ' ,  STATUS= ' UNKNOWN ' ) 
OPEN(31,FILE='liplot.rob5' ,  STATUS=' UNKNOWN' ) 

OPEN ( 32 , FILE= ' itplot . rob5 ' ,  STATUS* ' UNKNOWN ' ) 

OPEN (33, FILE* ' tes  tdat . robS ' ,  STATUS* ' UNKNOWN ' ) 

OPEN (34, FILE* 'dotplot. robs ',  STATUS* 'UNKNOWN' ) 
OPEN(35,FILE='posplot.rob5',  STATUS* ' UNKNOWN ' ) 

OPEN (36. FILE* ' alplot . robS ' ,  STATUS* ' UNKNOWN ' ) 

OPEN (37, FILE* 'ulplot. robs ' ,  STATUS* 'UNKNOWN' ) 

PI*  3.141S92654 

PRINT  •, 'ENTER  ITMAX,  CONV,  SLOWC' 

READ  *,  ITMAX,  CONV,  SLOWC 
PRINT  *, 'ENTER  ALPHAMIN' 

READ  *,  ALPHA  MIN 

print  *, 'dummy  read  for  compatibility  ,  enter  #' 

READ  '.DUMMY 

PRINT  *,  'ENTER  C,  WEIGHTING  PARAMTER  FOR  TERMINAL  COST' 
READ  *,  CWT 

PRINT  ♦, 'ENTER  EPSILON  for  f(deltheta)' 

READ  *,  EPS 

PRINT  *, 'ENTER  EPSILON  2  for  g(Ul)' 

READ  *,  EPS2 

PRINT  ♦,  'ENTER  INITIAL  X,Y,  AND  THETA (degrees ) ' 

READ  *,  XO.YO.THETAO 
THETA0=THETA0'PI/180 

PRINT  *,  'ENTER  FINAL  X,Y,  AND  THETA (degrees ) ' 

READ  *,  XF,YF,THETAD 
THETAD*THETAD*  PI / 1 8  0 

PRINT  *, 'ENTER  INITIAL  GUESS  FOR  3  LAMBDA  AMPLITUDES' 

READ  ♦,  RL1,RL2,RL3 

PRINT  *,  'ENTER  INITIAL  GUESS  FOR  3  LAMBDA  FREQUENCIES' 
READ  *.  OHMl,OHM2,OHM3 

PRINT  *,  'ENTER  INITIAL  GUESS  FOR  3  LAMBDA  DC  OFFSETS' 

READ  *,  IX:l,DC2,DC3 

PRINT  * , ' ENTER  INITIAL  GUESS  FOR  3  LAMBDA  PHASES ' 

READ  ♦,  PHIl,  PHI2,  PHI3 
PHI1*PHI1'PI/180 
PHI2*PHI2'PI/180 
PHI3=PHI3'PI/180 

PRINT  *, 'ENTER  INITIAL  GUESS  FOR  SIZE  OF  3  LAMBDA  VALUES' 
READ  *,  SL1,SL2,SL3 

DELX0=(XF-X0) 

DELYO* (YF-YO) 

DELTHETO* (THETAD-THETAO ) 

H=l. / (M-l> 

C  NO  INDEX  CHANGES  NECESSARY 

C  Index  Scale  used  by  SOLVDE.FOR 

INDEXVd)  =1 
INDEXV(2) =2 
INDEXV(3) *3 
INDEXV(4) =4 
INDEXV(5)=5 
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INDEXV(6) =6 


C  INITIAL  GUESS  FOR  ALL  POINTS,  1  -  M 

C  Initialize  independent  vector  X  (time) 

DO  11  K=1,M 

X(K) = (K-1) *H 

11  CONTINUE 

C  INITIAL  GUESS: 

Enter  initial  values  for  all  meshpoints 

NOTE :  BOUNDARY  CONDITONS  FOR  Y  ( 1 )  -Y  (  3  )  ARE 

ENTERED  AT  POINTS  1  AND  M  DURING  THE  INITIAL  GUESS !  !  ! 
THESE  NUMBERS  MUST  COINCIDE  WITH  ANY  DESIRED  B.C.'.M 

WRITE (3 3,*)  'STRAIGHT  LINE  GUESS' 

DO  12  K=1,M 

y(l,K) =DELX0- (DELXO* (K-1) / (M-1) ) 
y(2,K)=DELY0-(DELY0*(K-l)/(M-l) ) 
y(3,K) =DELTHET0- (DELTHETO* (K-1)/ (M-l) ) 
y(4,K) =RL1*SIN(2*PI*0HM1* (K-1) / (M-l) ♦PHI1)+DC1 
y(5,K)=RL2*SIN(2*PI*OHM2*(K-l)/(M-l)+PHI2)*DC2 
y(6,K)=RL3*SIN(2*PI*OHM3*  (K-l)/(M-l)+PHI3)tIX:3 

12  CONTINUE 

INITIAL  ALPHA  DETERMINATION: 


IF(Y(3, 1) .GT.EPS)  THEN 

FY3=  SIN(Y(3,1))/(Y(3,1)) 

ELSE 

FY3=  1.0 
ENDIF 

U10=Y(4, 1) *COS(THETAO-Y(3, 1) )  ♦  Y (5, 1) ‘SIN (THETAD- Y ( 3 , 1 ) ) 
&  ♦  Y(6,l)*(Y(2,l)*COS(THETAD)-Y(l,l)*SIN(THETAD))*FY3 

IF(ABS(U10) .GT.EPS2)THEN 
GU1=1.0 
ELSE 

GU1»0.0 

ENDIF 

BETA=Y(6, 1) ‘GUl'Y (3, 1) 

IF ( BETA . LT . ALPHAMIN) THEN 
ALPHA ( 1 ) = ALPHAMIN 

ELSE 

ALPHA ( 1 ) =BETA 
ENDIF 


C  Write  initial  guess  to  file 

DO  13  K=1,M 

WRITE  (30, 80)  X(K)  ,Y(1,K)  ,  Y(2,K)  ,Y(3,K) 

WRITE  (31, 80)  X(K)  ,Y(4,  K)  ,  Y(5,K)  ,  Y(€,K) 

13  CONTINUE 

C  Scalv  set  to  approximate  size  of  typical  values  of  luiown  solution 

SCALV ( 1 ) = ABS ( DELXO / 2 ) ♦ . 0 1 
SCALV ( 2 ) = ABS ( DELY 0 / 2 ) ♦ . 0 1 
SCALV ( 3 ) =ABS ( 2 'DELTHETO /M ) *.01 
SCALV(4) =ABS(SL1) ♦ .01 
SCALV ( 5 ) =ABS ( SL2 ) ♦ . 0 1 
SCALV(6) =ABS(SL3) * .01 
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WRITE  TEST  DATA  TO  FILE 
WRITE(33,*)  'ITMAX  =',ITMAX 
WRITE(33,*)  'CONV  =',CONV 
WRITE(33,*)  'SLOWC  = ' , SLOWC 
WRITE(33,*)  'ALPHA  MIN  =',ALPHAMIN 
WRITE(33,*)  'CWT  =',CWT 
WRITE(33,*)  'EPSILON  =',EPS 
WRITE(33,*)  'EPSILON  2  = ' , EPS2 

WRITE(33,*)  'INITIAL  X,Y  {>  THETA  =  ',  XO ,  YO ,  (THETA0*180/PI ) 
WRITE(33,*)  'FINAL  X,Y  i>  THETA  =  ',  XF,  YF,  tTHETAD*180/Pl ) 
WRITE  (33,*)  'INITIAL  LAMBDA  VALUE  AMPLITUDE',  RL1,RL2,RL3 
WRITE (3 3,*)  'SIZE  OF  LAMBDA  VALUES',  SL1,SL2,SL3 
WRITE{33,*)  'DELXO  =',  DELXO 
WRITE(33,*)  'DELYO  =',  DELYO 
WRITE (3 3,*)  'DELTHETO  =',  DELTHETO 

CALL  SOLVDE  ( ITMAX ,  CONV ,  SLOWC ,  SCALV ,  INDEXV ,  NE ,  NB ,  M ,  Y ,  NY J ,  NYK , 
*  C,NCI,NCJ,NCK, S,NSI,NSJ) 

C  Write  final  Y  values  to  file: 

DO  181  k  =  1,M 

WRITE (2 1,80)  X(K)  ,Y(l,k)  ,Y(2,)c)  ,Y(3,lc) 

WRITE (22, 80)  X (K)  ,  Y (4 ,  k)  ,  Y ( 5 , k) , Y (6 , k) 

WRITE(36,81)  X(K) ,ALPHA(K) 

181  CONTINUE 

80  FORMAT (2X,4F15. 4) 

81  FORMAT (2X,2F15. 4) 

CALL  XLATOR5(Y,  YDOT,H,NYJ,  NYK,  X,THETAD,  EPS,  ALPHA, 

&  XO , YO , THETAO , XF , YF , POS , EPS2 , UlTRAJ ) 

PRINT  • , ' PROGRAM  COMPLETED ' 

CLOSE (21) 

CLOSE(22) 

CLOSE(30) 

CLOSEOl) 

CLOSE (32) 

CLOSE(33) 

CLOSE (34) 

CLOSE (35) 

CLOSE (36) 

CLOSE (37) 

END 
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SUBROUTINE  DIFEQ ( K, K1 , K2 , JSF , ISl , ISF , INDEXV, NE , S , NSI , NS J , Y , NYJ , 
•  NYK) 

IMPLICIT  REAL*8  (A-H,  0-Z) 


•  MODIFIED  7/18/94  TO  INCLUDE  TERMINAL  COST  AT  FINA  BOUNDARY  CONDITION 

*  MODIFIED  7/21/94  FOR  NON-MOVING  VIRTUAL  (TARGET)  ROBOT 

*  MODIFIED  7/25/94  FOR  FUNCTION,  G(U1)  TIMES  ALPHA  TERM  IN  U2 . 

♦  (FEEDBACK  REFINEMENT) 

*  MODIFIED  7/26/94  FOR  ALPHA  AS  INPUT 

•  MODIFIED  7/26/94  FOR  PROPORTIONAL  ALPHA  AS  INPUT 


PARAMETER (M=201) 

DIMENSION  Y( NYJ, NYK) ,  S(NSI,NSJ),  INDEXV(NYJ) 

COMMON  X(M),  H,  DELXO ,  DELYO ,  DELTHETO,  THETAD,  EPS,  CWT , ALPHA (M) , 
&  EPS2 ,  ALPHAMIN 

C  Initialize  matrix  S  as  0 

DO  10  1=1, NSI 

DO  9  J=1,NSJ 

S(I,J)  =  0.0 

9  CONTINUE 

10  CONTINUE 


C  Initial  Boundary  Conditions 

IF(K.EQ.Kl)  THEN 

C  Enter  non-zero  values: 

DO  11  1=  1,3 

S(3*I,6'>-1)=1.0 
11  CONTINUE 

C  Initial  values  in  right  hand  vector  for  initial  blocic 


S(4, JSF)=  y(l, 1)-DELX0 
S(5, JSF) =  y( 2, 1) -DELYO 
S(6,JSF)=  y ( 3, 1) -DELTHETO 


C  End  Boundary  Conditions 

ELSE  IF  (K.GT.K2)  THEN 

C  Enter  non-zero  values: 

S(l,7)=  CWT 
S(l,8)=  0.0 
S(l,9)=  0.0 
S(l,10)=  -1.0 
S(l,ll)=  0.0 
S(l,12)=  0.0 

S(2,7) =  0.0 
S ( 2 , 8 ) =  CWT 
S(2,9)=  0.0 
S(2,10)=  0.0 
S(2,ll)=  -1.0 
S(2,12)=  0.0 

S(3,7)=  0.0 
S(3,8)=  0.0 
S(3,9)=  CWT 
S(3, 10) =  0.0 
S(3,ll)=  0.0 
S(3,12)=  -1.0 
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C  Final  values  in  right  hand  vector  for  final  block 

Sa,JSF)=  Y(1,M)*CWT  -  Y(4,M) 

S(2,JSF)=  Y{2,M)*CWT  -  Y(5,M) 

S(3,JSF)=  Y{3,M)*CWT  -  Y(6,K) 


C  Interior  Points 

C  Derived  from  Finite  Difference  Equations  of  Motion 


ELSE 

C  Pre-calculation  of  commonly  used  variables: 

Y1=(Y(1,K)+Y(1,K-1) ) /2.0 
Y2=(Y(2,K)+Y(2.K-1) )/2.0 
Y3=(Y(3,K)+Y(3,K-l))/2.0 
Y4=(Y(4,K)+Y(4,K-1) )/2.0 
Y5=(Y(5,K)+Y(5,K-1) ) /2.0 
Y6=(Y(6,K)+Y(6,K-1) ) /2.0 

CTD=COS (THETAD) 

STD=SIN(THETAD) 

CTDY3=C0S (THETAD-Y3 ) 

STDY3=SIN(THETAD-Y3) 

P=  Y2*CTD-Y1*STD 

IF(ABS(Y3) .GT.EPS)THEN 
FY3=SIN(Y3) / (Y3) 

FPY3=(COS(Y3) /Y3) -(SIN(Y3) / (Y3**2) ) 
SIG4=FPY3/2 .0 

SIG12=(-SIN(Y3) /Y3-2.0*COS(Y3) / (Y3) *•2  ♦ 

&  2.0*SIN(Y3)/(Y3)**3)/2.0 

ELSE 

FY3=1.0 

FPY3=0.0 

SIG4=0.0 

SIG12=-l/6.0 

ENDIF 

U1=Y4*CTDY3  *  Y5*STDY3  ♦  Y6*P*FY3 

IF(ABS(U1) .GT.EPS2)THEN 
GU1=1.0 
ELSE 

GU1=0.0 

ENDIF 

BETA=Y6*GU1*Y3 
I F ( BETA , LT . ALPHAMIN ) THEN 
ALPHA(K) =ALPHAMIN 
DELAL3=0 .0 
DELAL6=0.0 

ELSE 

ALPHA(K) =BETA 
DELAL3=Y6/2 
DELAL6=Y3/2 
ENDIF 

SIG1=  -Y6*FY3*STD/2 . 0 
SIG2=  Y6*FY3*CTD/2 .0 

SIG3=  Y4/2.0*STDY3  -  Y5/2.0*CTDY3  *  Y6*P*SIG4 


SIG6=  CTDY3/2.0 

SIG8=  STDY3/2.0 
SIG9=  P*FY3/2.0 
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SIGIO  =  -STD/2.0 
SIG11=  CTD/2.0 

c  Enter  non-zero  values: 

S(l,l)=  -1  +  H*CTDY3*SIG1 
S(l,2)=  H*CTDY3*SIG2 

S(l,3)=  H*(CTDY3*SIG3  +  STDY3 *U1 /2 . 0) 

S(l,4)=  H*(CTDY3*SIG6) 

S(l,5)=  H* (CTDY3*SIGB) 

S(l,6)=  H*CTDY3*SIG9 
S(l,7)=  S(l,l)  *  2.0 
S(l,8)=  S(l,2) 

S(l,9)=  S(l,3) 

S(l,10)=  S(l,4) 

S(l,ll)=  S(l,5) 

S(l,12)=  S(l,6) 

S(2,l)=  H*STDY3*SIG1 

S(2,2)=  -1  ♦  H*STDY3*SIG2 

S(2,3)=  H*tSTDY3*SIG3  -  CTDY3*Ul/2 .0) 

S(2,4)=  H*(STDY3*SIG6) 

S(2,5)=  H*(STDY3*SIG8) 

S(2,6)=  H*STDY3*SIG9 
S(2,7)=  S(2,l) 

S(2,8)=  S{2,2)  *  2.0 
S(2,9)=  S(2,3) 

S(2,10)=  S{2,4) 

S(2,ll)=  S(2,5) 

S(2,12)-  SI2,6) 

S(3,l)=  H*FY3* (P«SIG1+SIG10*U1) 

S(3,2)=  H*FY3* (P*SIG2+SIG11*U1> 

S(3,3)=  -1  ♦  H* (GUI* (ALPHA (K)/2.0  ♦  DELAL3*Y3) 
{,  ♦  P*(U1*SIG4  +  SIG3*FY3)) 

S(3,4)=  H*P*FY3*SIG6 
S(3,5)=  H*P*FY3*SIG8 

S(3, 6) =  H* (P*FY3*SIG9*Y3*GU1*DELAL6) 

S(3,7)=  S(3,l) 

S(3<8)=  S(3,2) 

S(3,9)=  S(3,3)  ♦  2.0 
S(3,10)=  S(3,4) 

S(3,ll)=  S(3,5) 

S(3,12l=  S{3,6) 

S(4,l)=  H*Y6*STD*FY3*S1G1 
S(4,2)=  H*Y6*STD*FY3*SIG2 
S(4,3)=  H*Y6*STD* (U1*SIG4  ♦  SIG3*FY3) 

S(4,4)=  -1  ♦  H*Y6*STD*FY3*SIG6 
S(4,5)=  H*Y6*STD*FY3*S1G8 
S(4,6)=  H*STD*FY3* (Y6*SIG9  ♦  Ul/2.0) 

S(4,7)=  S(4,l) 

S(4,8)=  S(4,2) 

S(4,9)=  S(4,3) 

S(4,10(=  S(4,4)  ♦  2,0 
S(4,ll)=  S(4,5) 

S(4,12)=  S(4,6) 

S(5,l)=  -H*Y6*CTD*FY3*SIG1 
S(5,2)=  -H*Y6*CTD*FY3*SIG2 
S(5,3)=  -H*Y6*CTD* (U1*SIG4  ♦  SIG3*FY3) 

S(5,4)=  -H*Y6*CTD*SIG6*FY3 
S(5,5)=  -1  -  H*Y6*CTD*SIG8*FY3 
S(5,6)=  -H*CTD*FY3* (Y6*SIG9  ♦  Ul/2.0) 

S(5,7)=  S(5,l) 

S(5,8)=  S(5,2) 

S(5,9)=  S(5,3) 

S(5,10)=  S(5,4) 

S(5.11)=  S(5,5I  ♦  2.0 
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n  n 


S(5,12)=  S(5,6) 


& 

& 

& 

& 

& 

& 

& 

Sc 


S(6,l)=  -H* (Y4*STDY3*SIG1  -  Y5*CTDY3*SIG1  + 

Y6*FPY3* (P*SIG1+SIG10*01) ) 

S(6,2)=  -H*(Y4*STDY3*SIG2  -  Y5*CTDY3*SIG2  ♦ 

Y6*FPY3* (P*SIG2+SIG11*01) ) 

S(6,3)=  -H*(Y4*(-Ul*CTDY3/2.0  +  SIG3*STDY3) 

-  Y5*(Ul*STDY3/2.0  ♦  CTDY3*SIG3) 

♦  Y6*P* (U1*SIG12  ♦  SIG3*FPY3  ♦  DELAL3*GU1)) 
S(6,4)=  -H* (STDY3* (Y4*SIG6  +  Ul/2.0)  -  Y5*CTDY3*SIG6  + 
Y6*P*FPY3*SIG6) 

S(6,5)=  -H*(Y4*STDY3*SIG8  -  CTDY3* (Y5*SIG8  ♦  Ul/2.0)  + 
Y6*P*FPY3*SIG8) 

S(6,6)=  -1  -H’ (Y4*STDY3*SIG9  -  Y5*CTDY3 *SIG9 
♦  Y6*  (P*FPY3*SIG9  ♦  DELAI,6*GU1) 

+  (ALPHA (K) *GU3  +P*U1*FPY3 ) /2 . 0) 

S(6,7)=  S(6,l) 

S(6,8)=  S(6,2) 

S(6,9)=  S<6,3) 

S(6.10)=  S(6,4) 

S(6,ll)=  S(6,5) 

S(6,12)=  S(6,6)  ♦  2.0 


S. 


S(l, JSF)=  Y(1,K) -Y(1,K-1)*H*(CTDY3*U1) 

S(2,JSF)=  Y(2,K)-Y{2,K-1)»H*(STDY3*U1) 

S(3, JSF) =  Y(3,K) -Y(3,K-1) ♦H*(ALPHA(K) *GUl*Y3tP*Ul *FY3 ) 
S(4, JSF) =  Y(4,K) -Y{4,K-1)+H*Y6*U1*STD*FY3 
S(5, JSF)=  Y(5,K)-Y{5,K-1)-H*Y6*01*CTD*FY3 
S(6, JSF) =  Y(6,K) -Y{6,K-1)-H* (Y4*U1*STDY3-Y5*U1*CTDY3+ 
Y6* (ALPHA(K) *GU1+P*U1*FPY3) ) 


ENDIF 

Dummy  use  of  variables  to  prevent  inocculous  warning  on  MS  Compiler 
(Variables  not  used) 

ISl  =  ISl 
ISF  =  ISF 

INDEXV(l)  =  INDEXV(l) 

NE  2  NE 

RETURN 

END 


149 


SUBROUTINE  XLATORS  ( Y,  YIXDT,  H.  NYJ,  NYK,  X,  THETAD,  EPS,  ALPHA, 
S>  XO ,  YO,  THETAO ,  XF,  YF,  POS ,  EPS2  ,  UlTRAJ ) 


MODIFIED  7/21/94  FOR  FIXED  VIRTUAL  (TARGET)  ROBOT  PROBLEM 
SUCH  THAT  UD=0  FOR  ALL  TIME 

MODIFIED  7/25/94  FOR  FUNCTION,  G(01)  TIMES  ALPHA  TERM  IN  U2 . 
(FEEDBACK  REFINEMENT) 

MODIFIED  7/26/94  FOR  PROPRTIONAL  ALPHA  CONTROL 


IMPLICIT  REAL’S  (A-H,  0-Z) 

PARAMETER (M=201) 

DIMENSION  Y(NYJ,NYK) ,  YDOT(5,M),  X(M),  POS ( 5 , M) , ALPHA (M) , 
&  COST  (M)  ,  NRG  (M)  ,  UlTRAJ  (M)  ,GU1TRAJ(M) 

POS ( 1 , 1 ) =X0 
POS(2, 1) =Y0 
POS(3, 1)=THETA0 
POS(4, 1)=XF 
POS(5, 1) =YF 

NRG  ( 1 )  =0 

DO  10  K=1,M 

IF(ABS(Y(3,K) ) .GT.EPS)THEN 
FY3=  SIN(Y(3,K) ) /Y(3,K) 

ELSE 

FY3=  1.0 
ENDIF 

Pdel=  Y(2,K) •COS(THETAD)-Y(l,K) •SIN(THETAD) 

Ul=  Y(4,K) •COS(THETAD-Y(3,K) )  ♦ 
fc  Y(5,K) •SIN(THETAD-Y(3,K) )  ♦ 

4  Y(6,K) ’Pdel’FYl 

UlTRAJ (K)=U1 

COST  FUNCTION  FOR  ROBOTS  ONLY 
NRG(K)=(U1TRAJ(K)’*2  +  ALPHA(K) **2) /2 
COST  FUNCTION  IN  SAME  FORM  AS  ROBOTS  FOR  COMPARISON: 
NRG(K)  =  (UlTRAJ(K)”2)/2  +  ALPHA(K) 

IF(ABS(U1) .GT.EPS2)THEN 
GU1=1.0 
ELSE 

GU1=0.0 

ENDIF 

GUITRAJ(K) =GU1 
UD=  0.0 

YDOT(l,K)=  COS(THETAD-Y(3,K) ) ’Ul 
YDOT(2,K)=  SIN(THETAD-Y(3,K) ) *U1 
YDOT(3,K)=  Pdel*Ul*FY3  ♦  ALPHA(K) *GU1’Y (3 , K) 

YDOT(4,K)=  COS(THETAD) *UD 
YDOT(5,K)=  SIN(THETAD) ’UD 

IF(K.GT.1)THEN 

POS(l,K)=XF-Y(l,K) 

POS(2,K)=YF-Y(2,K) 

POS ( 3 , K) =THETAD-Y ( 3 , K) 

POS(4,K)=XF 

POS(5,K)=YF 


TRAPEZOIDAL  INTEGRATION: 


C0ST(K)=C0ST(K-1)+H* (NRG (K) +NRG(K-1 ) ) /2 
ENDIF 


WRITE (34, 80)  X(K) ,YDOT(l.K) ,YDOT(2,K) ,YDOT(3,K) ,YDOT(4,K) , 
U  YDOT(5,K) 

WRITE (35, 80)  X(K) ,POS(l,K) , POS (2 , K) , POS (3 , K) , POS(4,K) , 

Ic  POS(S,K) 

WRITE(37,81)  X(K) ,U1TRAJ(K) ,CU1TRAJ(K) ,COST(K) 


10 


CONTINUE 


80  FORMAT(2X,6F20.10) 

81  FORMAT (2X,4F20. 10) 


RETURN 

END 
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APPENDIX  G 


Program  Sxibroutines  Used  by  All  Progr2uns 

SOLVDE . FOR 
PINVS . FOR 
RED . FOR 
BKSUB . FOR 
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subroutine  solvde (itmax, conv, slowc, scalv, indexv, ne, nb,m, 
y . nyj , nyk, c, nci, ncj , nek, s, nsi , nsj ) 

implicit  real's  (a-h,  o-z) 

PARAMETER  (NMAX=8) 

dimension  y (nyj , nyk) , c (nci , ncj , nek) , s (nsi, ns j ) 
dimension  scalv (nyj ), indexv (nyj ) 
dimension  ermax (NKAX) , kmax (NMAX) 


kl  =  1 
k2  =  m 

nvars  =  ne'm 
jl  =  1 
j2  =  nb 
j3  =  nb  +  1 
j4  =  ne 
jS  =  j4  +  jl 
j6  =  j4  ♦  j2 
j7  =  j4  ♦  j3 
jS  =  j4  +  j4 
39  =  38  +  jl 
icl  =  1 
ic2  =  ne  -  nb 
ic3  =  ic2  ♦  1 
ic4  =  ne 
jcl  =  1 
jef  =  ic3 

do  16  it  °  1, itmax 
k  =  kl 

call  difeq(k, kl,k2, j9, ic3, ic4, indexv, ne,  s, nsi, nsj ,y, nyj , nyk) 
call  pinvs (ic3 , ic4, j5, j9, jcl, kl,c,nci,ncj ,nck, s, nsi, ns j ) 
do  11  k  =  kl+l,k2 
kp  =  k  -  1 

call  di£eq(k, kl, k2, j9, icl, ic4, indexv, ne,  s, nsi, nsj ,y,nyj . nyk) 
call  red (icl, ic4, jl,j2,j3,j4,j9, ic3, jcl, jef , kp, c, nci, nc j ,nck, 
s , nsi , nsj ) 

call  pinvs (icl, ic4 , j3, j9, jcl, k, c, nci, ncj , nek, s, nsi , nsj ) 
continue 
k  =  k2  +  1 

call  dif€q(k, kl , k2, j9, icl , ic2 , indexv,  ne,  s,  nsi , ns j , y, nyj  ,  nyk) 

call  red ( icl, ic2 ,j5,j6,j7,j8,j9,ic3,jcl,jcf,k2,c,nci,ncj,nck, 
s, nsi, ns j ) 

call  pinvs (icl,ic2,j7,j9,jcf. k2  +  l,c,nci, ncj , nek, s , nsi , ns j ) 

call  bksub(ne, nh, jef , kl, k2 , c, nci, ncj  ,  nek) 

err  =  0.0 

do  13  j  =  l,ne 

jv  =  indexv(j) 

errj  =0.0 

)aii  =  0 . 0 

vmax  =  0.0 

do  12  k  =  kl,k2 

vz  =  abs (c( j , 1, k) ) 

if (vz .gt . vmax)  then 

vmax  =  vz 

km  =  k 

endif 

errj  =  errj  ♦  vz 
continue 

err  =  err  +  err j /scalv( jv) 


ennax(j)  =  c(  j  ,  1,  km) /scalv(  jv) 
kmax(j)  =  km 
continue 
err  =  err/nvars 
fac  =  slowc/max(slowc,err) 
do  15  jv  =  l,ne 
j  =  indexv(jv) 
do  14  k  =  kl,k2 

y(j,k)  =  y(j,k)  -  fac*c( jv,l,k) 
continue 
continue 

WRITE  (*,  101)  IT,ERR,y  (5, 1)  .Y(6,  1),Y(7,1),Y(8,1) 

WRITE (32, 101)  IT,  ERR,  Yd,  100)  ,  Y(2, 100)  ,  Y(3, 100)  ,  Y(4, 100) 
&  Y(5, 100) ,Y(6,100) , Y(7,100) , Y(8, 100) 

write(* , 100)  it, err, fac, (kmax( j ) , ermax( j ) , j=l, ne) 
WRITE(*,80)  IT,  ERR 
WRITE(32,80)  IT,  ERR 
FORMAT (2X, 110, F20.8) 
if (err . It .conv)  then 

write(33,*)  'last  iteration:  ' , it 
return 

endif 

continue 

WRITE (3 3,*)  ' itmax  exceeded' 
format (IX, 110, IX, 9f 15 . 6) 
format (lx, i4 , 2 f 12 . 6 , ( /5x, i5, f 12 . 6) ) 
return 


o  n 


subroutine  pinvs ( iel, ie2 ,jel,jsf,jcl,k,c,nci,ncj, nek, s , nsi , ns j ) 
C 

implicit  real's  (a-h,  o-z) 

C 

PARAMETER  (ZERO=0.,  ONE=l . ,  NMAX=8) 
dimension  c (nci, ncj , nek) ,  s(nsi,nsj) 
dimension  pscl (NMAX) , indxr (NMAX) 


je2  =  jel+ie2-iel 
jsl  =  je2+l 
do  12  i  =  iel,ie2 
big  =  zero 
do  11  j  =  jel,je2 

if (abs (s ( i, j ) ) .gt .big)  big  =  abs(s(i,j)) 

11  continue 

if (big. eq. zero)  pause  'Singular  matrix,  rows  all  0' 
pscl(i)  =  one/big 
indxr ( i )  =0 

12  continue 

do  16  id  =  iel,ie2 

piv  =  zero 

do  14  i  s  iel,ie2 

if (indxr (i) .eq.O)  then 

big  =  zero 

do  13  j  =  jel,je2 

if (abs (s ( i, j ) ) .gt . big)  then 

jp  =  j 

big  =  abs(s(i, j) ) 
endif 

13  continue 

if (big'pscl ( i) .gt .piv)  then 

ipiv  =  i 

jpiv  =  jp 

piv  =  big'pscl (i) 

endif 

endif 

14  continue 

if (s (ipiv, jpiv) .eq. zero)  pause  'Singular  matrix' 

indxr (ipiv)  =  jpiv 

pivinv  =  one/s ( ipiv, jpiv) 

do  15  j  =jel,jsf 

s(ipiv,j)  =  s ( ipiv, j) 'pivinv 

15  continue 

s (ipiv, jpiv)  =  one 

do  17  i  siel,ie2 

if ( indxr (i) .ne . jpiv)  then 

if (s ( i , jpiv) .ne . zero)  then 

dum  =  s(i,jpiv) 

do  16  j  =jel,jsf 

s(i,j)  =  s(i,j)  -  dum's ( ipiv, j ) 


16 

continue 

3(i, jpiv) 

endif 

endif 

17 

continue 

18 

continue 

jeoff  =  jcl-jsl 
icoff  =  iel-jel 
do  21  i=iel,ie2 
irow  =  indxr (i)  ♦  icoff 
do  19  j  =  jsl,jsf 
c ( irow, j* jeoff , k)  =  s(i,j) 
19  continue 

21  continue 

return 
end 
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subroutine  red  (izl ,  iz2 ,  jzl,  jz2,  jml,  jm2,  jmf,icl,jcl,jcf,)tc, 
c , nci , nc j , nek, s, nsi ,  ns j ) 

implicit  real’s  (a-h,  o-z) 

dimension  c (nci, nc j , nek) ,  s(nsi,nsi) 

loff  =  jcl-jml 
ic  =  id 

do  14  j  =  jzl,jz2 
do  12  1  =  jml,jm2 
vx  =  c ( ic. I’lof £ , kc) 
do  11  i  =  izl,iz2 
s(i,l)  =  s(i,i)  -  s(i,j)’vx 
continue 
continue 

vx  =  c(ic.jcf,kc) 
do  13  i  =  izl,iz2 
s(i,jmf)  =  s(i,jmf)  -  s(i,j)*vx 
continue 
ic  =  ic  +  1 
continue 
return 
end 


subroutine  bksub (ne, nb, j  f . kl ,  k2 , c. nci , ncj , nek) 
C 

implicit  real's  (a-h,  o-z) 

C 

dimension  c (nci , nc j , nek) 
nbf  =  ne-nb 
im  =  1 

do  13  k  =  k2,  kl,  -1 
if(k.eq.kl)  im  =  nbf+1 
kp  =  k  +  1 
do  12  j  =  l.nbf 
XX  =  c  ( j  ,  j  f ,  kp) 
do  11  i  =  im.ne 

c(i.jf,k)  =  c(i,jf,k)  -  c(i.j,k)*xx 


11 

continue 

12 

continue 

13 

continue 

do  16  k  =  kl,k2 

)cp  =  k  ♦  1 

do  14  i  =  l,nb 

c(I,l,k)  =  c  (i+nbf ,  j  £ ,  k) 

14  continue 

do  15  i  =  l.nbf 
c(i+nb,l,k)  =  c(i,]f,kp) 

15  continue 

16  continue 
return 
end 
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